add support for dynamic_reconfigure

Author: Kai Franke

CMakeLists.txt

@@ -9,13 +9,18 @@ find_package(catkin REQUIRED
   rospy
   sensor_msgs
   tf
+  dynamic_reconfigure
 )
 catkin_python_setup()
 
 # ROS message generation
 add_message_files(DIRECTORY msg FILES Axis.msg)
 generate_messages(DEPENDENCIES geometry_msgs std_msgs)
 
+generate_dynamic_reconfigure_options(
+  cfg/PTZ.cfg
+)
+
 catkin_package(
   CATKIN_DEPENDS
   camera_info_manager_py
@@ -32,3 +37,7 @@ install(PROGRAMS
   nodes/teleop.py
   nodes/teleop_speed_control.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
+install(DIRECTORY
+  cfg
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

cfg/PTZ.cfg

@@ -0,0 +1,16 @@
+#!/usr/bin/env python
+PACKAGE = "axis_camera"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+#gen.add("speed_control", bool_t, 0, "Speed control (true) or absolute position control (false)", False)
+gen.add("autofocus", bool_t, 0, "Autofocus", True)
+gen.add("pan", double_t, 0, "Pan value in degrees (or percent in absolute mode)", 0, -180, 180)
+gen.add("tilt", double_t, 0, "Tilt value in degrees (or percent in absolute mode)", -45, -90, 0)
+gen.add("zoom", double_t, 0, "Zoom value", 1, 1, 9999)
+gen.add("focus", double_t, 0, "Focus", 1, 1, 9999)
+gen.add("brightness", double_t, 0, "Brightness", 1, 1, 9999)
+
+exit(gen.generate(PACKAGE, "axis_camera", "PTZ"))

nodes/axis_ptz.py

@@ -9,6 +9,8 @@
 from axis_camera.msg import Axis
 from std_msgs.msg import Bool
 import math
+from dynamic_reconfigure.server import Server
+from axis_camera.cfg import PTZConfig
 
 class StateThread(threading.Thread):
     '''This class handles the publication of the positional state of the camera 
@@ -237,6 +239,32 @@ def createCmdString(self):
     def mirrorCallback(self, msg):
         '''Command the camera with speed control or position control commands'''
         self.mirror = msg.data
+        
+    def callback(self, config, level):
+        #self.speedControl = config.speed_control
+        
+        # create temporary message and fill with data from dynamic reconfigure
+        temp_msg = Axis()
+        temp_msg.pan = config.pan
+        temp_msg.tilt = config.tilt
+        temp_msg.zoom = config.zoom
+        temp_msg.focus = config.focus
+        temp_msg.brightness = config.brightness
+        temp_msg.autofocus = config.autofocus
+        
+        # check sanity and apply values
+        self.cmd(temp_msg)
+        
+        # read sanitized values and update GUI
+        config.pan = self.msg.pan
+        config.tilt = self.msg.tilt
+        config.zoom = self.msg.zoom
+        config.focus = self.msg.focus
+        config.brightness = self.msg.brightness
+        config.autofocus = self.msg.autofocus
+        
+        # update GUI with sanitized values
+        return config
 
 def main():
     rospy.init_node("axis_twist")
@@ -253,7 +281,9 @@ def main():
         args[name] = rospy.get_param(rospy.search_param(name), 
                                                             arg_defaults[name])
 
-    AxisPTZ(**args)
+    # create new PTZ object and start dynamic_reconfigure server
+    my_ptz = AxisPTZ(**args)
+    srv = Server(PTZConfig, my_ptz.callback)
     rospy.spin()
 
 if __name__ == "__main__":