diff --git a/ur_driver/CMakeLists.txt b/ur_driver/CMakeLists.txt index 7a7e1057572b52fd5c79623c92a0027ceb0c377c..17790bf54bf66f48899118ed2f3e697d5378e524 100644 --- a/ur_driver/CMakeLists.txt +++ b/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 ## ############# diff --git a/ur_driver/cfg/URDriver.cfg b/ur_driver/cfg/URDriver.cfg new file mode 100755 index 0000000000000000000000000000000000000000..404d58ab607edbd1a04c5444228a47a744cb5d18 --- /dev/null +++ b/ur_driver/cfg/URDriver.cfg @@ -0,0 +1,10 @@ +#!/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")) diff --git a/ur_driver/package.xml b/ur_driver/package.xml index f72b2252900c2a4ee65fba15a1e63b7589171575..ec898fdeb4a533fd00bff6cad5ea1ae82155d59e 100644 --- a/ur_driver/package.xml +++ b/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> diff --git a/ur_driver/src/ur_driver/driver.py b/ur_driver/src/ur_driver/driver.py index 631804f6a3d291c5c0eaae1af70b23c3b374fafe..1134ea799d21a3dc99b351e3c63fd1e52115901e 100755 --- a/ur_driver/src/ur_driver/driver.py +++ b/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)