Initial commit

sensors/velodyne/velodyne_driver/src/driver/CMakeLists.txt

@@ -0,0 +1,26 @@
+# build the driver node
+add_executable(velodyne_node velodyne_node.cc driver.cc)
+add_dependencies(velodyne_node velodyne_driver_gencfg)
+target_link_libraries(velodyne_node
+  velodyne_input
+  ${catkin_LIBRARIES}
+  ${libpcap_LIBRARIES}
+)
+
+# build the nodelet version
+add_library(driver_nodelet nodelet.cc driver.cc)
+add_dependencies(driver_nodelet velodyne_driver_gencfg)
+target_link_libraries(driver_nodelet
+  velodyne_input
+  ${catkin_LIBRARIES}
+  ${libpcap_LIBRARIES}
+)
+
+# install runtime files
+install(TARGETS velodyne_node
+        RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+        COMPONENT main
+)
+install(TARGETS driver_nodelet
+        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)

sensors/velodyne/velodyne_driver/src/driver/driver.cc

@@ -0,0 +1,297 @@
+// Copyright (C) 2007, 2009-2012 Austin Robot Technology, Patrick Beeson, Jack O'Quin
+// All rights reserved.
+//
+// Software License Agreement (BSD License 2.0)
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above
+//    copyright notice, this list of conditions and the following
+//    disclaimer in the documentation and/or other materials provided
+//    with the distribution.
+//  * Neither the name of {copyright_holder} nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+/** \file
+ *
+ *  ROS driver implementation for the Velodyne 3D LIDARs
+ */
+
+#include <string>
+#include <cmath>
+
+#include <ros/ros.h>
+#include <tf/transform_listener.h>
+#include <velodyne_msgs/VelodyneScan.h>
+
+#include "velodyne_driver/driver.h"
+
+namespace velodyne_driver
+{
+
+VelodyneDriver::VelodyneDriver(ros::NodeHandle node,
+                               ros::NodeHandle private_nh,
+                               std::string const & node_name)
+  : diagnostics_(node, private_nh, node_name)
+{
+  // use private node handle to get parameters
+  private_nh.param("frame_id", config_.frame_id, std::string("velodyne"));
+  std::string tf_prefix = tf::getPrefixParam(private_nh);
+  ROS_DEBUG_STREAM("tf_prefix: " << tf_prefix);
+  config_.frame_id = tf::resolve(tf_prefix, config_.frame_id);
+
+  // get model name, validate string, determine packet rate
+  private_nh.param("model", config_.model, std::string("64E"));
+  double packet_rate;                   // packet frequency (Hz)
+  std::string model_full_name;
+  if ((config_.model == "64E_S2") || 
+      (config_.model == "64E_S2.1"))    // generates 1333312 points per second
+    {                                   // 1 packet holds 384 points
+      packet_rate = 3472.17;            // 1333312 / 384
+      model_full_name = std::string("HDL-") + config_.model;
+    }
+  else if (config_.model == "64E")
+    {
+      packet_rate = 2600.0;
+      model_full_name = std::string("HDL-") + config_.model;
+    }
+  else if (config_.model == "64E_S3") // generates 2222220 points per second (half for strongest and half for lastest)
+    {                                 // 1 packet holds 384 points
+      packet_rate = 5787.03;          // 2222220 / 384
+      model_full_name = std::string("HDL-") + config_.model;
+    }
+  else if (config_.model == "32E")
+    {
+      packet_rate = 1808.0;
+      model_full_name = std::string("HDL-") + config_.model;
+    }
+    else if (config_.model == "32C")
+    {
+      packet_rate = 1507.0;
+      model_full_name = std::string("VLP-") + config_.model;
+    }
+  else if (config_.model == "VLP16")
+    {
+      packet_rate = 754;             // 754 Packets/Second for Last or Strongest mode 1508 for dual (VLP-16 User Manual)
+      model_full_name = "VLP-16";
+    }
+  else
+    {
+      ROS_ERROR_STREAM("unknown Velodyne LIDAR model: " << config_.model);
+      packet_rate = 2600.0;
+    }
+  std::string deviceName(std::string("Velodyne ") + model_full_name);
+
+  private_nh.param("rpm", config_.rpm, 600.0);
+  ROS_INFO_STREAM(deviceName << " rotating at " << config_.rpm << " RPM");
+  double frequency = (config_.rpm / 60.0);     // expected Hz rate
+
+  // default number of packets for each scan is a single revolution
+  // (fractions rounded up)
+  config_.npackets = (int) ceil(packet_rate / frequency);
+  private_nh.getParam("npackets", config_.npackets);
+  ROS_INFO_STREAM("publishing " << config_.npackets << " packets per scan");
+
+  // if we are timestamping based on the first or last packet in the scan
+  private_nh.param("timestamp_first_packet", config_.timestamp_first_packet, false);
+  if (config_.timestamp_first_packet)
+    ROS_INFO("Setting velodyne scan start time to timestamp of first packet");
+
+  std::string dump_file;
+  private_nh.param("pcap", dump_file, std::string(""));
+
+  double cut_angle;
+  private_nh.param("cut_angle", cut_angle, -0.01);
+  if (cut_angle < 0.0)
+  {
+    ROS_INFO_STREAM("Cut at specific angle feature deactivated.");
+  }
+  else if (cut_angle < (2*M_PI))
+  {
+      ROS_INFO_STREAM("Cut at specific angle feature activated. " 
+        "Cutting velodyne points always at " << cut_angle << " rad.");
+  }
+  else
+  {
+    ROS_ERROR_STREAM("cut_angle parameter is out of range. Allowed range is "
+    << "between 0.0 and 2*PI or negative values to deactivate this feature.");
+    cut_angle = -0.01;
+  }
+
+  // Convert cut_angle from radian to one-hundredth degree, 
+  // which is used in velodyne packets
+  config_.cut_angle = int((cut_angle*360/(2*M_PI))*100);
+
+  int udp_port;
+  private_nh.param("port", udp_port, (int) DATA_PORT_NUMBER);
+
+  // Initialize dynamic reconfigure
+  srv_ = boost::make_shared <dynamic_reconfigure::Server<velodyne_driver::
+    VelodyneNodeConfig> > (private_nh);
+  dynamic_reconfigure::Server<velodyne_driver::VelodyneNodeConfig>::
+    CallbackType f;
+  f = boost::bind (&VelodyneDriver::callback, this, _1, _2);
+  srv_->setCallback (f); // Set callback function und call initially
+
+  // initialize diagnostics
+  diagnostics_.setHardwareID(deviceName);
+  const double diag_freq = packet_rate/config_.npackets;
+  diag_max_freq_ = diag_freq;
+  diag_min_freq_ = diag_freq;
+  ROS_INFO("expected frequency: %.3f (Hz)", diag_freq);
+
+  using namespace diagnostic_updater;
+  diag_topic_.reset(new TopicDiagnostic("velodyne_packets", diagnostics_,
+                                        FrequencyStatusParam(&diag_min_freq_,
+                                                             &diag_max_freq_,
+                                                             0.1, 10),
+                                        TimeStampStatusParam()));
+  diag_timer_ = private_nh.createTimer(ros::Duration(0.2), &VelodyneDriver::diagTimerCallback,this);
+
+  config_.enabled = true;
+
+  // open Velodyne input device or file
+  if (dump_file != "")                  // have PCAP file?
+    {
+      // read data from packet capture file
+      input_.reset(new velodyne_driver::InputPCAP(private_nh, udp_port,
+                                                  packet_rate, dump_file));
+    }
+  else
+    {
+      // read data from live socket
+      input_.reset(new velodyne_driver::InputSocket(private_nh, udp_port));
+    }
+
+  // raw packet output topic
+  output_ =
+    node.advertise<velodyne_msgs::VelodyneScan>("velodyne_packets", 10);
+
+  last_azimuth_ = -1;
+}
+
+/** poll the device
+ *
+ *  @returns true unless end of file reached
+ */
+bool VelodyneDriver::poll(void)
+{
+  if (!config_.enabled) {
+    // If we are not enabled exit once a second to let the caller handle
+    // anything it might need to, such as if it needs to exit.
+    ros::Duration(1).sleep();
+    return true;
+  }
+
+  // Allocate a new shared pointer for zero-copy sharing with other nodelets.
+  velodyne_msgs::VelodyneScanPtr scan(new velodyne_msgs::VelodyneScan);
+
+  if( config_.cut_angle >= 0) //Cut at specific angle feature enabled
+  {
+    scan->packets.reserve(config_.npackets);
+    velodyne_msgs::VelodynePacket tmp_packet;
+    while(true)
+    {
+      while(true)
+      {
+        int rc = input_->getPacket(&tmp_packet, config_.time_offset);
+        if (rc == 0) break;       // got a full packet?
+        if (rc < 0) return false; // end of file reached?
+      }
+      scan->packets.push_back(tmp_packet);
+
+      // Extract base rotation of first block in packet
+      std::size_t azimuth_data_pos = 100*0+2;
+      int azimuth = *( (u_int16_t*) (&tmp_packet.data[azimuth_data_pos]));
+
+      //if first packet in scan, there is no "valid" last_azimuth_
+      if (last_azimuth_ == -1) {
+      	 last_azimuth_ = azimuth;
+      	 continue;
+      }
+      if((last_azimuth_ < config_.cut_angle && config_.cut_angle <= azimuth)
+      	 || ( config_.cut_angle <= azimuth && azimuth < last_azimuth_)
+      	 || (azimuth < last_azimuth_ && last_azimuth_ < config_.cut_angle))
+      {
+        last_azimuth_ = azimuth;
+        break; // Cut angle passed, one full revolution collected
+      }
+      last_azimuth_ = azimuth;
+    }
+  }
+  else // standard behaviour
+  {
+  // Since the velodyne delivers data at a very high rate, keep
+  // reading and publishing scans as fast as possible.
+    scan->packets.resize(config_.npackets);
+    for (int i = 0; i < config_.npackets; ++i)
+    {
+      while (true)
+        {
+          // keep reading until full packet received
+          int rc = input_->getPacket(&scan->packets[i], config_.time_offset);
+          if (rc == 0) break;       // got a full packet?
+          if (rc < 0) return false; // end of file reached?
+        }
+    }
+  }
+
+  // publish message using time of last packet read
+  ROS_DEBUG("Publishing a full Velodyne scan.");
+  if (config_.timestamp_first_packet){
+    scan->header.stamp = scan->packets.front().stamp;
+  }
+  else{
+    scan->header.stamp = scan->packets.back().stamp;
+  }
+  scan->header.frame_id = config_.frame_id;
+  output_.publish(scan);
+
+  // notify diagnostics that a message has been published, updating
+  // its status
+  diag_topic_->tick(scan->header.stamp);
+  diagnostics_.update();
+
+  return true;
+}
+
+void VelodyneDriver::callback(velodyne_driver::VelodyneNodeConfig &config,
+              uint32_t level)
+{
+  ROS_INFO("Reconfigure Request");
+  if (level & 1)
+  {
+    config_.time_offset = config.time_offset;
+  }
+  if (level & 2)
+  {
+    config_.enabled = config.enabled;
+  }
+}
+
+void VelodyneDriver::diagTimerCallback(const ros::TimerEvent &event)
+{
+  (void)event;
+  // Call necessary to provide an error when no velodyne packets are received
+  diagnostics_.update();
+}
+
+} // namespace velodyne_driver

sensors/velodyne/velodyne_driver/src/driver/nodelet.cc

@@ -0,0 +1,109 @@
+// Copyright (C) 2012 Austin Robot Technology, Jack O'Quin
+// All rights reserved.
+//
+// Software License Agreement (BSD License 2.0)
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above
+//    copyright notice, this list of conditions and the following
+//    disclaimer in the documentation and/or other materials provided
+//    with the distribution.
+//  * Neither the name of {copyright_holder} nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+/** \file
+ *
+ *  ROS driver nodelet for the Velodyne 3D LIDARs
+ */
+
+#include <string>
+#include <boost/thread.hpp>
+
+#include <ros/ros.h>
+#include <pluginlib/class_list_macros.h>
+#include <nodelet/nodelet.h>
+
+#include "velodyne_driver/driver.h"
+
+namespace velodyne_driver
+{
+
+class DriverNodelet: public nodelet::Nodelet
+{
+public:
+
+  DriverNodelet():
+    running_(false)
+  {}
+
+  ~DriverNodelet()
+  {
+    if (running_)
+      {
+        NODELET_INFO("shutting down driver thread");
+        running_ = false;
+        deviceThread_->join();
+        NODELET_INFO("driver thread stopped");
+      }
+  }
+
+private:
+
+  virtual void onInit(void);
+  virtual void devicePoll(void);
+
+  volatile bool running_;               ///< device thread is running
+  boost::shared_ptr<boost::thread> deviceThread_;
+
+  boost::shared_ptr<VelodyneDriver> dvr_; ///< driver implementation class
+};
+
+void DriverNodelet::onInit()
+{
+  // start the driver
+  dvr_.reset(new VelodyneDriver(getNodeHandle(), getPrivateNodeHandle(), getName()));
+
+  // spawn device poll thread
+  running_ = true;
+  deviceThread_ = boost::shared_ptr< boost::thread >
+    (new boost::thread(boost::bind(&DriverNodelet::devicePoll, this)));
+}
+
+/** @brief Device poll thread main loop. */
+void DriverNodelet::devicePoll()
+{
+  while(ros::ok())
+    {
+      // poll device until end of file
+      running_ = dvr_->poll();
+      if (!running_)
+        ROS_ERROR_THROTTLE(1.0, "DriverNodelet::devicePoll - Failed to poll device.");
+    }
+  running_ = false;
+}
+
+} // namespace velodyne_driver
+
+// Register this plugin with pluginlib.  Names must match nodelet_velodyne.xml.
+//
+// parameters are: class type, base class type
+PLUGINLIB_EXPORT_CLASS(velodyne_driver::DriverNodelet, nodelet::Nodelet)

sensors/velodyne/velodyne_driver/src/driver/velodyne_node.cc

@@ -0,0 +1,62 @@
+// Copyright (C) 2012 Austin Robot Technology, Jack O'Quin
+// All rights reserved.
+//
+// Software License Agreement (BSD License 2.0)
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above
+//    copyright notice, this list of conditions and the following
+//    disclaimer in the documentation and/or other materials provided
+//    with the distribution.
+//  * Neither the name of {copyright_holder} nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+/** \file
+ *
+ *  ROS driver node for the Velodyne 3D LIDARs.
+ */
+
+#include <ros/ros.h>
+#include "velodyne_driver/driver.h"
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "velodyne_node");
+  ros::NodeHandle node;
+  ros::NodeHandle private_nh("~");
+
+  // start the driver
+  velodyne_driver::VelodyneDriver dvr(node, private_nh);
+
+  // loop until shut down or end of file
+  while(ros::ok())
+    {
+      // poll device until end of file
+      bool polled_ = dvr.poll();
+      if (!polled_)
+        ROS_ERROR_THROTTLE(1.0, "Velodyne - Failed to poll device.");
+
+      ros::spinOnce();
+    }
+
+  return 0;
+}

sensors/velodyne/velodyne_driver/src/lib/CMakeLists.txt

@@ -0,0 +1,12 @@
+add_library(velodyne_input input.cc)
+target_link_libraries(velodyne_input
+  ${catkin_LIBRARIES}
+  ${libpcap_LIBRARIES}
+)
+if(catkin_EXPORTED_TARGETS)
+  add_dependencies(velodyne_input ${catkin_EXPORTED_TARGETS})
+endif()
+
+install(TARGETS velodyne_input
+        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)

sensors/velodyne/velodyne_driver/src/lib/input.cc

@@ -0,0 +1,356 @@
+// Copyright (C) 2007, 2009, 2010, 2015 Austin Robot Technology, Patrick Beeson, Jack O'Quin
+// All rights reserved.
+//
+// Software License Agreement (BSD License 2.0)
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above
+//    copyright notice, this list of conditions and the following
+//    disclaimer in the documentation and/or other materials provided
+//    with the distribution.
+//  * Neither the name of {copyright_holder} nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+/** \file
+ *
+ *  Input classes for the Velodyne HDL-64E 3D LIDAR:
+ *
+ *     Input -- base class used to access the data independently of
+ *              its source
+ *
+ *     InputSocket -- derived class reads live data from the device
+ *              via a UDP socket
+ *
+ *     InputPCAP -- derived class provides a similar interface from a
+ *              PCAP dump
+ */
+
+#include <unistd.h>
+#include <string>
+#include <sstream>
+#include <sys/socket.h>
+#include <arpa/inet.h>
+#include <poll.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <sys/file.h>
+#include <velodyne_driver/input.h>
+#include <velodyne_driver/time_conversion.hpp>
+
+namespace velodyne_driver
+{
+  static const size_t packet_size =
+    sizeof(velodyne_msgs::VelodynePacket().data);
+
+  ////////////////////////////////////////////////////////////////////////
+  // Input base class implementation
+  ////////////////////////////////////////////////////////////////////////
+
+  /** @brief constructor
+   *
+   *  @param private_nh ROS private handle for calling node.
+   *  @param port UDP port number.
+   */
+  Input::Input(ros::NodeHandle private_nh, uint16_t port):
+    private_nh_(private_nh),
+    port_(port)
+  {
+    private_nh.param("device_ip", devip_str_, std::string(""));
+    private_nh.param("gps_time", gps_time_, false);
+    if (!devip_str_.empty())
+      ROS_INFO_STREAM("Only accepting packets from IP address: "
+                      << devip_str_);
+  }
+
+  ////////////////////////////////////////////////////////////////////////
+  // InputSocket class implementation
+  ////////////////////////////////////////////////////////////////////////
+
+  /** @brief constructor
+   *
+   *  @param private_nh ROS private handle for calling node.
+   *  @param port UDP port number
+   */
+  InputSocket::InputSocket(ros::NodeHandle private_nh, uint16_t port):
+    Input(private_nh, port)
+  {
+    sockfd_ = -1;
+    
+    if (!devip_str_.empty()) {
+      inet_aton(devip_str_.c_str(),&devip_);
+    }    
+
+    // connect to Velodyne UDP port
+    ROS_INFO_STREAM("Opening UDP socket: port " << port);
+    sockfd_ = socket(PF_INET, SOCK_DGRAM, 0);
+    if (sockfd_ == -1)
+      {
+        perror("socket");               // TODO: ROS_ERROR errno
+        return;
+      }
+  
+    sockaddr_in my_addr;                     // my address information
+    memset(&my_addr, 0, sizeof(my_addr));    // initialize to zeros
+    my_addr.sin_family = AF_INET;            // host byte order
+    my_addr.sin_port = htons(port);          // port in network byte order
+    my_addr.sin_addr.s_addr = INADDR_ANY;    // automatically fill in my IP
+  
+    if (bind(sockfd_, (sockaddr *)&my_addr, sizeof(sockaddr)) == -1)
+      {
+        perror("bind");                 // TODO: ROS_ERROR errno
+        return;
+      }
+  
+    if (fcntl(sockfd_,F_SETFL, O_NONBLOCK|FASYNC) < 0)
+      {
+        perror("non-block");
+        return;
+      }
+
+    ROS_DEBUG("Velodyne socket fd is %d\n", sockfd_);
+  }
+
+  /** @brief destructor */
+  InputSocket::~InputSocket(void)
+  {
+    (void) close(sockfd_);
+  }
+
+  /** @brief Get one velodyne packet. */
+  int InputSocket::getPacket(velodyne_msgs::VelodynePacket *pkt, const double time_offset)
+  {
+    double time1 = ros::Time::now().toSec();
+
+    struct pollfd fds[1];
+    fds[0].fd = sockfd_;
+    fds[0].events = POLLIN;
+    static const int POLL_TIMEOUT = 1000; // one second (in msec)
+
+    sockaddr_in sender_address;
+    socklen_t sender_address_len = sizeof(sender_address);
+
+    while (true)
+      {
+        // Unfortunately, the Linux kernel recvfrom() implementation
+        // uses a non-interruptible sleep() when waiting for data,
+        // which would cause this method to hang if the device is not
+        // providing data.  We poll() the device first to make sure
+        // the recvfrom() will not block.
+        //
+        // Note, however, that there is a known Linux kernel bug:
+        //
+        //   Under Linux, select() may report a socket file descriptor
+        //   as "ready for reading", while nevertheless a subsequent
+        //   read blocks.  This could for example happen when data has
+        //   arrived but upon examination has wrong checksum and is
+        //   discarded.  There may be other circumstances in which a
+        //   file descriptor is spuriously reported as ready.  Thus it
+        //   may be safer to use O_NONBLOCK on sockets that should not
+        //   block.
+
+        // poll() until input available
+        do
+          {
+            int retval = poll(fds, 1, POLL_TIMEOUT);
+            if (retval < 0)             // poll() error?
+              {
+                if (errno != EINTR)
+                  ROS_ERROR("poll() error: %s", strerror(errno));
+                return -1;
+              }
+            if (retval == 0)            // poll() timeout?
+              {
+                ROS_WARN("Velodyne poll() timeout");
+                return -1;
+              }
+            if ((fds[0].revents & POLLERR)
+                || (fds[0].revents & POLLHUP)
+                || (fds[0].revents & POLLNVAL)) // device error?
+              {
+                ROS_ERROR("poll() reports Velodyne error");
+                return -1;
+              }
+          } while ((fds[0].revents & POLLIN) == 0);
+
+        // Receive packets that should now be available from the
+        // socket using a blocking read.
+        ssize_t nbytes = recvfrom(sockfd_, &pkt->data[0],
+                                  packet_size,  0,
+                                  (sockaddr*) &sender_address,
+                                  &sender_address_len);
+
+        if (nbytes < 0)
+          {
+            if (errno != EWOULDBLOCK)
+              {
+                perror("recvfail");
+                ROS_INFO("recvfail");
+                return -1;
+              }
+          }
+        else if ((size_t) nbytes == packet_size)
+          {
+            // read successful,
+            // if packet is not from the lidar scanner we selected by IP,
+            // continue otherwise we are done
+            if(devip_str_ != ""
+               && sender_address.sin_addr.s_addr != devip_.s_addr)
+              continue;
+            else
+              break; //done
+          }
+
+        ROS_DEBUG_STREAM("incomplete Velodyne packet read: "
+                         << nbytes << " bytes");
+      }
+
+    if (!gps_time_) {
+      // Average the times at which we begin and end reading.  Use that to
+      // estimate when the scan occurred. Add the time offset.
+      double time2 = ros::Time::now().toSec();
+      pkt->stamp = ros::Time((time2 + time1) / 2.0 + time_offset);
+    } else {
+      // time for each packet is a 4 byte uint located starting at offset 1200 in
+      // the data packet
+      pkt->stamp = rosTimeFromGpsTimestamp(&(pkt->data[1200]));
+    }
+
+    return 0;
+  }
+
+  ////////////////////////////////////////////////////////////////////////
+  // InputPCAP class implementation
+  ////////////////////////////////////////////////////////////////////////
+
+  /** @brief constructor
+   *
+   *  @param private_nh ROS private handle for calling node.
+   *  @param port UDP port number
+   *  @param packet_rate expected device packet frequency (Hz)
+   *  @param filename PCAP dump file name
+   */
+  InputPCAP::InputPCAP(ros::NodeHandle private_nh, uint16_t port,
+                       double packet_rate, std::string filename,
+                       bool read_once, bool read_fast, double repeat_delay):
+    Input(private_nh, port),
+    packet_rate_(packet_rate),
+    filename_(filename)
+  {
+    pcap_ = NULL;  
+    empty_ = true;
+
+    // get parameters using private node handle
+    private_nh.param("read_once", read_once_, false);
+    private_nh.param("read_fast", read_fast_, false);
+    private_nh.param("repeat_delay", repeat_delay_, 0.0);
+
+    if (read_once_)
+      ROS_INFO("Read input file only once.");
+    if (read_fast_)
+      ROS_INFO("Read input file as quickly as possible.");
+    if (repeat_delay_ > 0.0)
+      ROS_INFO("Delay %.3f seconds before repeating input file.",
+               repeat_delay_);
+
+    // Open the PCAP dump file
+    ROS_INFO("Opening PCAP file \"%s\"", filename_.c_str());
+    if ((pcap_ = pcap_open_offline(filename_.c_str(), errbuf_) ) == NULL)
+      {
+        ROS_FATAL("Error opening Velodyne socket dump file.");
+        return;
+      }
+
+    std::stringstream filter;
+    if( devip_str_ != "" )              // using specific IP?
+      {
+        filter << "src host " << devip_str_ << " && ";
+      }
+    filter << "udp dst port " << port;
+    pcap_compile(pcap_, &pcap_packet_filter_,
+                 filter.str().c_str(), 1, PCAP_NETMASK_UNKNOWN);
+  }
+
+  /** destructor */
+  InputPCAP::~InputPCAP(void)
+  {
+    pcap_close(pcap_);
+  }
+
+  /** @brief Get one velodyne packet. */
+  int InputPCAP::getPacket(velodyne_msgs::VelodynePacket *pkt, const double time_offset)
+  {
+    struct pcap_pkthdr *header;
+    const u_char *pkt_data;
+
+    while (true)
+      {
+        int res;
+        if ((res = pcap_next_ex(pcap_, &header, &pkt_data)) >= 0)
+          {
+            // Skip packets not for the correct port and from the
+            // selected IP address.
+            if (0 == pcap_offline_filter(&pcap_packet_filter_,
+                                          header, pkt_data))
+              continue;
+
+            // Keep the reader from blowing through the file.
+            if (read_fast_ == false)
+              packet_rate_.sleep();
+            
+            memcpy(&pkt->data[0], pkt_data+42, packet_size);
+            pkt->stamp = ros::Time::now(); // time_offset not considered here, as no synchronization required
+            empty_ = false;
+            return 0;                   // success
+          }
+
+        if (empty_)                 // no data in file?
+          {
+            ROS_WARN("Error %d reading Velodyne packet: %s", 
+                     res, pcap_geterr(pcap_));
+            return -1;
+          }
+
+        if (read_once_)
+          {
+            ROS_INFO("end of file reached -- done reading.");
+            return -1;
+          }
+        
+        if (repeat_delay_ > 0.0)
+          {
+            ROS_INFO("end of file reached -- delaying %.3f seconds.",
+                     repeat_delay_);
+            usleep(rint(repeat_delay_ * 1000000.0));
+          }
+
+        ROS_DEBUG("replaying Velodyne dump file");
+
+        // I can't figure out how to rewind the file, because it
+        // starts with some kind of header.  So, close the file
+        // and reopen it with pcap.
+        pcap_close(pcap_);
+        pcap_ = pcap_open_offline(filename_.c_str(), errbuf_);
+        empty_ = true;              // maybe the file disappeared?
+      } // loop back and try again
+  }
+
+} // velodyne namespace

sensors/velodyne/velodyne_driver/src/vdump

@@ -0,0 +1,23 @@
+#!/bin/bash
+
+# dump velodyne packets
+# $Id: vdump 8892 2009-10-24 15:13:57Z joq $
+
+if [ x$1 = x ]
+then	echo -e "usage:\t`basename $0` file-prefix [ interface ]"
+	echo -e "\n\tfile-prefix is completed with a three-digit number"
+	echo -e "\tinterface default is 'eth0'\n"
+	exit 9
+fi
+
+IF=${2:-eth0}
+UN=`id -un`
+ID=`id -u`
+
+echo "acquiring packets on $IF for user $UN; press ^C when done"
+
+if [ $ID = 0 ]; then
+  /usr/sbin/tcpdump -i $IF -s 0 -C 100 -W 999 -w $1
+else
+  sudo /usr/sbin/tcpdump -i $IF -Z $UN -s 0 -C 100 -W 999 -w $1
+fi