Merge pull request #151 from Axel13fr/feature/No_Communication_Diag_U…

…pdate

Raise diagnostic when no packets are received

velodyne_driver/src/driver/driver.cc

@@ -129,6 +129,7 @@ VelodyneDriver::VelodyneDriver(ros::NodeHandle node,
                                                              &diag_max_freq_,
                                                              0.1, 10),
                                         TimeStampStatusParam()));
+  diag_timer_ = private_nh.createTimer(ros::Duration(0.2), &VelodyneDriver::diagTimerCallback,this);
 
   // open Velodyne input device or file
   if (dump_file != "")                  // have PCAP file?
@@ -228,4 +229,11 @@ void VelodyneDriver::callback(velodyne_driver::VelodyneNodeConfig &config,
   config_.time_offset = config.time_offset;
 }
 
+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

velodyne_driver/src/driver/driver.h

@@ -42,6 +42,8 @@ class VelodyneDriver
   ///Callback for dynamic reconfigure
   void callback(velodyne_driver::VelodyneNodeConfig &config,
               uint32_t level);
+  /// Callback for diagnostics update for lost communication with vlp
+  void diagTimerCallback(const ros::TimerEvent&event);
 
   ///Pointer to dynamic reconfigure service srv_
   boost::shared_ptr<dynamic_reconfigure::Server<velodyne_driver::
@@ -62,6 +64,7 @@ class VelodyneDriver
   ros::Publisher output_;
 
   /** diagnostics updater */
+  ros::Timer diag_timer_;
   diagnostic_updater::Updater diagnostics_;
   double diag_min_freq_;
   double diag_max_freq_;