Feature/add frequency tolerance parameter (#469)

* Add frequency tolerance parameter

* Add frequency tolerance variable

* Set configured topic tolerance in diagnostics

* Remove parameter from dynamic config struct

* Remove parameter from dynamic config

* Make frequency tolerance a regular parameter

* Remove leftover variable assigment

* Update parameter name

* Add parameter to launch

Co-authored-by: Cesar Lopez <[email protected]>

Author: cesar-lopez-mar

velodyne_driver/launch/nodelet_manager.launch

@@ -17,6 +17,7 @@
   <arg name="pcap_time" default="false" />
   <arg name="cut_angle" default="-0.01" />
   <arg name="timestamp_first_packet" default="false" />
+  <arg name="diagnostic_frequency_tolerance" default="0.1" />
 
   <!-- start nodelet manager -->
   <node pkg="nodelet" type="nodelet" name="$(arg manager)" args="manager" output="screen"/>
@@ -37,6 +38,7 @@
     <param name="pcap_time" value="$(arg pcap_time)"/>
     <param name="cut_angle" value="$(arg cut_angle)"/>
     <param name="timestamp_first_packet" value="$(arg timestamp_first_packet)"/>
-  </node>    
+    <param name="diagnostic_frequency_tolerance" value="$(arg diagnostic_frequency_tolerance)"/>
+  </node>
 
 </launch>

velodyne_driver/src/driver/driver.cc

@@ -133,7 +133,7 @@ VelodyneDriver::VelodyneDriver(ros::NodeHandle node,
   }
   else if (cut_angle < (2*M_PI))
   {
-      ROS_INFO_STREAM("Cut at specific angle feature activated. " 
+      ROS_INFO_STREAM("Cut at specific angle feature activated. "
         "Cutting velodyne points always at " << cut_angle << " rad.");
   }
   else
@@ -143,7 +143,7 @@ VelodyneDriver::VelodyneDriver(ros::NodeHandle node,
     cut_angle = -0.01;
   }
 
-  // Convert cut_angle from radian to one-hundredth degree, 
+  // 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);
 
@@ -165,11 +165,13 @@ VelodyneDriver::VelodyneDriver(ros::NodeHandle node,
   diag_min_freq_ = diag_freq;
   ROS_INFO("expected frequency: %.3f (Hz)", diag_freq);
 
+  double diagnostic_frequency_tolerance;
+  private_nh.param("diagnostic_frequency_tolerance", diagnostic_frequency_tolerance, 0.1);
   using namespace diagnostic_updater;
   diag_topic_.reset(new TopicDiagnostic("velodyne_packets", diagnostics_,
                                         FrequencyStatusParam(&diag_min_freq_,
                                                              &diag_max_freq_,
-                                                             0.1, 10),
+                                                             diagnostic_frequency_tolerance, 10),
                                         TimeStampStatusParam()));
   diag_timer_ = private_nh.createTimer(ros::Duration(0.2), &VelodyneDriver::diagTimerCallback,this);