dynamic reconfigure server for prevent_programming

ur_driver/CMakeLists.txt

@@ -4,17 +4,20 @@ project(ur_driver)
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
-find_package(catkin REQUIRED)
+find_package(catkin REQUIRED dynamic_reconfigure)
 
 catkin_python_setup()
 
 
+generate_dynamic_reconfigure_options(
+  cfg/URDriver.cfg
+)
+
 ###################################
 ## catkin specific configuration ##
 ###################################
 catkin_package()
 
-
 #############
 ## Install ##
 #############

ur_driver/cfg/URDriver.cfg

+#!/usr/bin/env python
+PACKAGE = "ur_driver"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("prevent_programming",   bool_t,   0, "Prevent driver from continuously uploading 'prog'",  False)
+
+exit(gen.generate(PACKAGE, "ur_driver", "URDriver"))

ur_driver/package.xml

@@ -15,6 +15,8 @@
   <url type="website">http://ros.org/wiki/ur_driver</url>
 
   <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
+  
   <run_depend>rospy</run_depend>
   <run_depend>actionlib</run_depend>
   <run_depend>control_msgs</run_depend>
@@ -22,4 +24,5 @@
   <run_depend>trajectory_msgs</run_depend>
   <run_depend>ur_msgs</run_depend>
   <run_depend>python-lxml</run_depend>
+  <run_depend>dynamic_reconfigure</run_depend>
 </package>

ur_driver/src/ur_driver/driver.py

@@ -16,6 +16,9 @@ from control_msgs.msg import FollowJointTrajectoryAction
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 from geometry_msgs.msg import WrenchStamped
 
+from dynamic_reconfigure.server import Server
+from ur_driver.cfg import URDriverConfig
+
 from ur_driver.deserialize import RobotState, RobotMode
 from ur_driver.deserializeRT import RobotStateRT
 
@@ -894,12 +897,24 @@ def handle_set_io(req):
 def set_io_server():
     s= rospy.Service('set_io', SetIO, handle_set_io)
 
+def reconfigure_callback(config, level):
+    global prevent_programming
+    prevent_programming = config.prevent_programming
+    ## What about updating the value on the parameter server?
+    return config
+
 def main():
     rospy.init_node('ur_driver', disable_signals=True)
     if rospy.get_param("use_sim_time", False):
         rospy.logwarn("use_sim_time is set!!!")
+    
     global prevent_programming
     prevent_programming = rospy.get_param("prevent_programming", False)
+    reconfigure_srv = Server(URDriverConfig, reconfigure_callback)
+    ## Still use parameter server?
+    #update = URDriverConfig(prevent_programming)
+    #reconfigure_srv.update_configuration(update)
+    
     prefix = rospy.get_param("~prefix", "")
     print "Setting prefix to %s" % prefix
     global joint_names
@@ -968,7 +983,7 @@ def main():
             # Checks for disconnect
             if getConnectedRobot(wait=False):
                 time.sleep(0.2)
-                prevent_programming = rospy.get_param("prevent_programming", False)
+                #prevent_programming = rospy.get_param("prevent_programming", False)
                 if prevent_programming:
                     print "Programming now prevented"
                     connection.send_reset_program()
@@ -983,7 +998,7 @@ def main():
                     while not connection.ready_to_program():
                         print "Waiting to program"
                         time.sleep(1.0)
-                    prevent_programming = rospy.get_param("prevent_programming", False)
+                    #prevent_programming = rospy.get_param("prevent_programming", False)
                     connection.send_program()
 
                     r = getConnectedRobot(wait=True, timeout=1.0)