Merge pull request #89 from Tones29/feat_dynrec_driver

Add dynamic latency configuration to velodyne_driver

velodyne_driver/CMakeLists.txt

@@ -3,6 +3,7 @@ project(velodyne_driver)
 
 set(${PROJECT_NAME}_CATKIN_DEPS 
     diagnostic_updater
+    dynamic_reconfigure
     nodelet
     roscpp
     tf
@@ -18,6 +19,9 @@ set(libpcap_LIBRARIES -lpcap)
 
 include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS})
 
+# Generate dynamic_reconfigure server
+generate_dynamic_reconfigure_options(cfg/VelodyneNode.cfg)
+
 # objects needed by other ROS packages that depend on this one
 catkin_package(CATKIN_DEPENDS ${${PROJECT_NAME}_CATKIN_DEPS}
                INCLUDE_DIRS include

velodyne_driver/cfg/VelodyneNode.cfg

@@ -0,0 +1,14 @@
+#!/usr/bin/env python
+PACKAGE = "velodyne_driver"
+NODE_NAME = "velodyne_node"
+PARAMS_NAME = "VelodyneNode"
+
+from math import pi
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("time_offset", double_t,  0, "A manually calibrated offset (in seconds) to add to the timestamp before publication of a message.",
+        0.0, -1.0, 1.0)
+
+exit(gen.generate(PACKAGE, NODE_NAME, PARAMS_NAME))

velodyne_driver/include/velodyne_driver/input.h

@@ -48,6 +48,7 @@ namespace velodyne_driver
   {
   public:
     Input() {}
+    virtual ~Input() {}
 
     /** @brief Read one Velodyne packet.
      *
@@ -57,7 +58,7 @@ namespace velodyne_driver
      *          -1 if end of file
      *          > 0 if incomplete packet (is this possible?)
      */
-    virtual int getPacket(velodyne_msgs::VelodynePacket *pkt) = 0;
+    virtual int getPacket(velodyne_msgs::VelodynePacket *pkt, const double time_offset) = 0;
 
 
     /** @brief Set source IP, from where packets are accepted
@@ -75,9 +76,9 @@ namespace velodyne_driver
   public:
     InputSocket(ros::NodeHandle private_nh,
                 uint16_t udp_port = UDP_PORT_NUMBER);
-    ~InputSocket();
+    virtual ~InputSocket();
 
-    virtual int getPacket(velodyne_msgs::VelodynePacket *pkt);
+    virtual int getPacket(velodyne_msgs::VelodynePacket *pkt, const double time_offset);
     void setDeviceIP( const std::string& ip );
   private:
 
@@ -100,9 +101,9 @@ namespace velodyne_driver
               bool read_once=false,
               bool read_fast=false,
               double repeat_delay=0.0);
-    ~InputPCAP();
+    virtual ~InputPCAP();
 
-    virtual int getPacket(velodyne_msgs::VelodynePacket *pkt);
+    virtual int getPacket(velodyne_msgs::VelodynePacket *pkt, const double time_offset);
     void setDeviceIP( const std::string& ip );
 
   private:

velodyne_driver/package.xml

@@ -20,6 +20,7 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <build_depend>diagnostic_updater</build_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
   <build_depend>libpcap</build_depend>
   <build_depend>nodelet</build_depend>
   <build_depend>pluginlib</build_depend>
@@ -32,6 +33,7 @@
   <build_depend>rostest</build_depend>
 
   <run_depend>diagnostic_updater</run_depend>
+  <run_depend>dynamic_reconfigure</run_depend>
   <run_depend>libpcap</run_depend>
   <run_depend>nodelet</run_depend>
   <run_depend>pluginlib</run_depend>

velodyne_driver/src/driver/CMakeLists.txt

@@ -1,5 +1,6 @@
 # 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}
@@ -8,6 +9,7 @@ target_link_libraries(velodyne_node
 
 # 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}

velodyne_driver/src/driver/driver.cc

@@ -81,6 +81,14 @@ VelodyneDriver::VelodyneDriver(ros::NodeHandle node,
   int udp_port;
   private_nh.param("port", udp_port, (int)UDP_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;
@@ -134,15 +142,15 @@ bool VelodyneDriver::poll(void)
       while (true)
         {
           // keep reading until full packet received
-          int rc = input_->getPacket(&scan->packets[i]);
+          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.");
-  scan->header.stamp = ros::Time(scan->packets[config_.npackets - 1].stamp);
+  scan->header.stamp = scan->packets[config_.npackets - 1].stamp;
   scan->header.frame_id = config_.frame_id;
   output_.publish(scan);
 
@@ -154,4 +162,11 @@ bool VelodyneDriver::poll(void)
   return true;
 }
 
+void VelodyneDriver::callback(velodyne_driver::VelodyneNodeConfig &config,
+              uint32_t level)
+{
+  ROS_INFO("Reconfigure Request");
+  config_.time_offset = config.time_offset;
+}
+
 } // namespace velodyne_driver

velodyne_driver/src/driver/driver.h

@@ -19,8 +19,10 @@
 #include <ros/ros.h>
 #include <diagnostic_updater/diagnostic_updater.h>
 #include <diagnostic_updater/publisher.h>
+#include <dynamic_reconfigure/server.h>
 
 #include <velodyne_driver/input.h>
+#include <velodyne_driver/VelodyneNodeConfig.h>
 
 namespace velodyne_driver
 {
@@ -37,13 +39,22 @@ class VelodyneDriver
 
 private:
 
+  ///Callback for dynamic reconfigure
+  void callback(velodyne_driver::VelodyneNodeConfig &config,
+              uint32_t level);
+
+  ///Pointer to dynamic reconfigure service srv_
+  boost::shared_ptr<dynamic_reconfigure::Server<velodyne_driver::
+              VelodyneNodeConfig> > srv_;
+
   // configuration parameters
   struct
   {
     std::string frame_id;            ///< tf frame ID
     std::string model;               ///< device model name
     int    npackets;                 ///< number of packets to collect
     double rpm;                      ///< device rotation rate (RPMs)
+    double time_offset;              ///< time in seconds added to each velodyne time stamp
   } config_;
 
   boost::shared_ptr<Input> input_;

velodyne_driver/src/lib/input.cc

@@ -91,7 +91,7 @@ namespace velodyne_driver
   }
 
   /** @brief Get one velodyne packet. */
-  int InputSocket::getPacket(velodyne_msgs::VelodynePacket *pkt)
+  int InputSocket::getPacket(velodyne_msgs::VelodynePacket *pkt, const double time_offset)
   {
     double time1 = ros::Time::now().toSec();
 
@@ -177,9 +177,9 @@ namespace velodyne_driver
       }
 
     // Average the times at which we begin and end reading.  Use that to
-    // estimate when the scan occurred.
+    // estimate when the scan occurred. Add the time offset.
     double time2 = ros::Time::now().toSec();
-    pkt->stamp = ros::Time((time2 + time1) / 2.0);
+    pkt->stamp = ros::Time((time2 + time1) / 2.0 + time_offset);
 
     return 0;
   }
@@ -248,7 +248,7 @@ namespace velodyne_driver
   }
 
   /** @brief Get one velodyne packet. */
-  int InputPCAP::getPacket(velodyne_msgs::VelodynePacket *pkt)
+  int InputPCAP::getPacket(velodyne_msgs::VelodynePacket *pkt, const double time_offset)
   {
     struct pcap_pkthdr *header;
     const u_char *pkt_data;
@@ -267,7 +267,7 @@ namespace velodyne_driver
               packet_rate_.sleep();
 
             memcpy(&pkt->data[0], pkt_data+42, packet_size);
-            pkt->stamp = ros::Time::now();
+            pkt->stamp = ros::Time::now(); // time_offset not considered here, as no synchronization required
             empty_ = false;
             return 0;                   // success
           }

velodyne_pointcloud/src/conversions/transform.cc

@@ -25,8 +25,8 @@ namespace velodyne_pointcloud
 {
   /** @brief Constructor. */
   Transform::Transform(ros::NodeHandle node, ros::NodeHandle private_nh):
-    data_(new velodyne_rawdata::RawData()),
-    tf_prefix_(tf::getPrefixParam(private_nh))
+    tf_prefix_(tf::getPrefixParam(private_nh)),
+    data_(new velodyne_rawdata::RawData())
   {
     // Read calibration.
     data_->setup(private_nh);