00001 # Software License Agreement (BSD License) 00002 # 00003 # Copyright (c) 2012, Willow Garage, Inc. 00004 # All rights reserved. 00005 # 00006 # Redistribution and use in source and binary forms, with or without 00007 # modification, are permitted provided that the following conditions 00008 # are met: 00009 # 00010 # * Redistributions of source code must retain the above copyright 00011 # notice, this list of conditions and the following disclaimer. 00012 # * Redistributions in binary form must reproduce the above 00013 # copyright notice, this list of conditions and the following 00014 # disclaimer in the documentation and/or other materials provided 00015 # with the distribution. 00016 # * Neither the name of Willow Garage, Inc. nor the names of its 00017 # contributors may be used to endorse or promote products derived 00018 # from this software without specific prior written permission. 00019 # 00020 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00021 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00022 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00023 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00024 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00025 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00026 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00027 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00028 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00029 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00030 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00031 # POSSIBILITY OF SUCH DAMAGE. 00032 00033 import fnmatch 00034 import rospy 00035 import threading 00036 00037 from json import loads, dumps 00038 00039 00040 """ Methods to interact with the param server. Values have to be passed 00041 as JSON in order to facilitate dynamically typed SRV messages """ 00042 00043 # rospy parameter server isn't thread-safe 00044 param_server_lock = threading.RLock() 00045 00046 def set_param(name, value, params_glob): 00047 if params_glob and not any(fnmatch.fnmatch(str(name), glob) for glob in params_glob): 00048 # If the glob list is not empty and there are no glob matches, 00049 # stop the attempt to set the parameter. 00050 return 00051 # If the glob list is empty (i.e. false) or the parameter matches 00052 # one of the glob strings, continue to set the parameter. 00053 d = None 00054 try: 00055 d = loads(value) 00056 except ValueError: 00057 raise Exception("Due to the type flexibility of the ROS parameter server, the value argument to set_param must be a JSON-formatted string.") 00058 with param_server_lock: 00059 rospy.set_param(name, d) 00060 00061 00062 def get_param(name, default, params_glob): 00063 if params_glob and not any(fnmatch.fnmatch(str(name), glob) for glob in params_glob): 00064 # If the glob list is not empty and there are no glob matches, 00065 # stop the attempt to get the parameter. 00066 return 00067 # If the glob list is empty (i.e. false) or the parameter matches 00068 # one of the glob strings, continue to get the parameter. 00069 d = None 00070 if default is not "": 00071 try: 00072 d = loads(default) 00073 except ValueError: 00074 d = default 00075 with param_server_lock: 00076 value = rospy.get_param(name, d) 00077 return dumps(value) 00078 00079 def has_param(name, params_glob): 00080 if params_glob and not any(fnmatch.fnmatch(str(name), glob) for glob in params_glob): 00081 # If the glob list is not empty and there are no glob matches, 00082 # stop the attempt to set the parameter. 00083 return False 00084 # If the glob list is empty (i.e. false) or the parameter matches 00085 # one of the glob strings, check whether the parameter exists. 00086 with param_server_lock: 00087 return rospy.has_param(name) 00088 00089 def delete_param(name, params_glob): 00090 if params_glob and not any(fnmatch.fnmatch(str(name), glob) for glob in params_glob): 00091 # If the glob list is not empty and there are no glob matches, 00092 # stop the attempt to delete the parameter. 00093 return 00094 # If the glob list is empty (i.e. false) or the parameter matches 00095 # one of the glob strings, continue to delete the parameter. 00096 if has_param(name, params_glob): 00097 with param_server_lock: 00098 rospy.delete_param(name) 00099 00100 def search_param(name, params_glob): 00101 if params_glob and not any(fnmatch.fnmatch(str(v), glob) for glob in params_glob): 00102 # If the glob list is not empty and there are no glob matches, 00103 # stop the attempt to find the parameter. 00104 return None 00105 # If the glob list is empty (i.e. false) or the parameter matches 00106 # one of the glob strings, continue to find the parameter. 00107 return rospy.search_param(name) 00108 00109 def get_param_names(params_glob): 00110 with param_server_lock: 00111 if params_glob: 00112 # If there is a parameter glob, filter by it. 00113 return filter(lambda x: any(fnmatch.fnmatch(str(x), glob) for glob in params_glob), rospy.get_param_names()) 00114 else: 00115 # If there is no parameter glob, don't filter. 00116 return rospy.get_param_names() 00117