[jsk_network_tools] Add dynamic_reconfigure interface to specify bandwidth of highspeed streamer

jsk_network_tools/CMakeLists.txt

@@ -1,7 +1,9 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(jsk_network_tools)
 
-find_package(catkin REQUIRED COMPONENTS message_generation std_msgs sensor_msgs rostest)
+find_package(catkin REQUIRED COMPONENTS
+  message_generation std_msgs sensor_msgs rostest
+  dynamic_reconfigure)
 catkin_python_setup()
 add_message_files(
   DIRECTORY msg
@@ -15,7 +17,9 @@ add_message_files(
   OpenNISample.msg
   AllTypeTest.msg
 )
-
+generate_dynamic_reconfigure_options(
+  cfg/SilverhammerHighspeedStreamer.cfg
+  )
 ## Generate added messages and services with any dependencies listed here
 generate_messages(
   DEPENDENCIES

jsk_network_tools/cfg/SilverhammerHighspeedStreamer.cfg

@@ -0,0 +1,20 @@
+#!/usr/bin/env python
+
+# set up parameters that we care about
+PACKAGE = 'jsk_network_tools'
+
+try:
+    import imp
+    imp.find_module(PACKAGE)
+    from dynamic_reconfigure.parameter_generator_catkin import *;
+except:
+    import roslib; roslib.load_manifest(PACKAGE)
+    from dynamic_reconfigure.parameter_generator import *;
+
+from math import pi
+
+gen = ParameterGenerator ()
+# 1kbps ~ 1Gbps
+gen.add("bandwidth", int_t, 0, "Expected bandwisth in bits", 250 * 1000 * 1000, 1000, 1000 * 1000 * 1000)
+
+exit (gen.generate (PACKAGE, PACKAGE, "SilverhammerHighspeedStreamer"))

jsk_network_tools/package.xml

@@ -17,10 +17,11 @@
   <build_depend>rostest</build_depend>
   <build_depend>diagnostic_msgs</build_depend>
   <build_depend>diagnostic_updater</build_depend>
-
+  <build_depend>dynamic_reconfigure</build_depend>
   <run_depend>std_msgs</run_depend>
   <run_depend>message_runtime</run_depend>
   <run_depend>sensor_msgs</run_depend>
   <run_depend>diagnostic_msgs</run_depend>
   <run_depend>diagnostic_updater</run_depend>
+  <run_depend>dynamic_reconfigure</run_depend>
 </package>

jsk_network_tools/scripts/silverhammer_highspeed_streamer.py

@@ -12,6 +12,8 @@
 import roslib
 from roslib.message import get_message_class
 from std_msgs.msg import Time
+from dynamic_reconfigure.server import Server
+from jsk_network_tools.cfg import SilverHammerStreamerConfig
 
 class SilverHammerStreamer:
     def __init__(self):
@@ -22,6 +24,7 @@ def __init__(self):
         except:
             raise Exception("invalid topic type: %s"%message_class_str)
         self.lock = Lock()
+        self.dynamic_reconfigure = Server(SilverHammerStreamerConfig, self.dynamicReconfigureCallback)
         self.launched_time = rospy.Time.now()
         self.packet_interval = None
         self.diagnostic_updater = diagnostic_updater.Updater()
@@ -36,8 +39,6 @@ def __init__(self):
         self.last_input_received_time_pub = rospy.Publisher(
             "~last_input_received_time", Time)
         self.send_num = 0
-        #self.packet_interval = rospy.get_param("~packet_interval", 0.001)
-        self.bandwidth = rospy.get_param("~bandwidth", 280 * 1000 * 1000)
         self.rate = rospy.get_param("~send_rate", 2)   #2Hz
         self.socket_client = socket(AF_INET, SOCK_DGRAM)
         self.packet_size = rospy.get_param("~packet_size", 1000)   #2Hz
@@ -49,6 +50,9 @@ def __init__(self):
                                       self.sendTimerCallback)
         self.diagnostic_timer = rospy.Timer(rospy.Duration(1.0 / 10),
                                             self.diagnosticTimerCallback)
+    def dynamicReconfigureCallback(self, config, level):
+        with self.lock:
+            self.bandwidth = config.bandwidth
     def diagnosticCallback(self, stat):
         # always OK
         stat.summary(DiagnosticStatus.OK, "OK")