From 62b9c5c74a56e17bf437f0efba003cab843479e0 Mon Sep 17 00:00:00 2001
From: ipa-fxm <felix.messmer@ipa.fhg.de>
Date: Sat, 14 Feb 2015 13:06:11 +0100
Subject: [PATCH] dynamic reconfigure server for prevent_programming
---
ur_driver/CMakeLists.txt | 7 +++++--
ur_driver/cfg/URDriver.cfg | 10 ++++++++++
ur_driver/package.xml | 3 +++
ur_driver/src/ur_driver/driver.py | 19 +++++++++++++++++--
4 files changed, 35 insertions(+), 4 deletions(-)
create mode 100755 ur_driver/cfg/URDriver.cfg
diff --git a/ur_driver/CMakeLists.txt b/ur_driver/CMakeLists.txt
index 7a7e105..17790bf 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 0000000..404d58a
--- /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 f72b225..ec898fd 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 631804f..1134ea7 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)
--
GitLab