Source Code for Module dynamic_reconfigure.server

  1  # Software License Agreement (BSD License) 
  2  # 
  3  # Copyright (c) 2009, Willow Garage, Inc. 
  4  # All rights reserved. 
  5  # 
  6  # Redistribution and use in source and binary forms, with or without 
  7  # modification, are permitted provided that the following conditions 
  8  # are met: 
  9  # 
 10  #  * Redistributions of source code must retain the above copyright 
 11  #    notice, this list of conditions and the following disclaimer. 
 12  #  * Redistributions in binary form must reproduce the above 
 13  #    copyright notice, this list of conditions and the following 
 14  #    disclaimer in the documentation and/or other materials provided 
 15  #    with the distribution. 
 16  #  * Neither the name of Willow Garage, Inc. nor the names of its 
 17  #    contributors may be used to endorse or promote products derived 
 18  #    from this software without specific prior written permission. 
 19  # 
 20  # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
 21  # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
 22  # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 
 23  # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 
 24  # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 
 25  # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 
 26  # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
 27  # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 
 28  # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 
 29  # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 
 30  # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
 31  # POSSIBILITY OF SUCH DAMAGE. 
 32   
 33  """ 
 34  Python client API for dynamic_reconfigure (L{DynamicReconfigureClient}) as well as 
 35  example server implementation (L{DynamicReconfigureServer}). 
 36  """ 
 37   
 38  from __future__ import with_statement 
 39   
 40  try: 
 41      import roslib; roslib.load_manifest('dynamic_reconfigure') 
 42  except: 
 43      pass 
 44  import rospy 
 45  import threading 
 46  import copy 
 47   
 48  from dynamic_reconfigure import DynamicReconfigureCallbackException 
 49  from dynamic_reconfigure.encoding import decode_config, encode_config, encode_description, extract_params, get_tree, initial_config 
 50  from dynamic_reconfigure.msg import Config as ConfigMsg 
 51  from dynamic_reconfigure.msg import ConfigDescription as ConfigDescrMsg 
 52  from dynamic_reconfigure.srv import Reconfigure as ReconfigureSrv 
 53   
 54   
 55 -class Server(object): 
 56 -    def __init__(self, type, callback, namespace=""): 
 57          self.mutex = threading.Lock() 
 58          if not namespace: 
 59              self.ns = "~" 
 60          else: 
 61              if namespace[0] not in ["/", "~"]: 
 62                  namespace = "~" + namespace 
 63              self.ns = (namespace + "/").replace("//", "/") 
 64   
 65          self.type = type 
 66          self.config = type.defaults.copy() 
 67   
 68          self.description = encode_description(type) 
 69          self._copy_from_parameter_server() 
 70          self.callback = callback 
 71          self._clamp(self.config) 
 72   
 73          # setup group defaults 
 74          self.config['groups'] = get_tree(self.description) 
 75          self.config = initial_config(encode_config(self.config), type.config_description) 
 76   
 77          self.descr_topic = rospy.Publisher(self.ns + 'parameter_descriptions', ConfigDescrMsg, latch=True, queue_size=10) 
 78          self.descr_topic.publish(self.description) 
 79   
 80          self.update_topic = rospy.Publisher(self.ns + 'parameter_updates', ConfigMsg, latch=True, queue_size=10) 
 81          self._change_config(self.config, ~0)  # Consistent with the C++ API, the callback gets called with level=~0 (i.e. -1) 
 82   
 83          self.set_service = rospy.Service(self.ns + 'set_parameters', ReconfigureSrv, self._set_callback) 
 84   
 85 -    def update_configuration(self, changes): 
 86          with self.mutex: 
 87              new_config = copy.deepcopy(self.config) 
 88              new_config.update(changes) 
 89              self._clamp(new_config) 
 90              return self._change_config(new_config, self._calc_level(new_config, self.config)) 
 91   
 92 -    def _copy_from_parameter_server(self): 
 93          for param in extract_params(self.type.config_description): 
 94              try: 
 95                  self.config[param['name']] = rospy.get_param(self.ns + param['name']) 
 96              except KeyError: 
 97                  pass 
 98   
 99 -    def _copy_to_parameter_server(self): 
100          for param in extract_params(self.type.config_description): 
101              rospy.set_param(self.ns + param['name'], self.config[param['name']]) 
102   
103 -    def _change_config(self, config, level): 
104          self.config = self.callback(config, level) 
105          if self.config is None: 
106              msg = 'Reconfigure callback should return a possibly updated configuration.' 
107              rospy.logerr(msg) 
108              raise DynamicReconfigureCallbackException(msg) 
109   
110          self._copy_to_parameter_server() 
111   
112          self.update_topic.publish(encode_config(self.config)) 
113   
114          return self.config 
115   
116 -    def _calc_level(self, config1, config2): 
117          level = 0 
118          for param in extract_params(self.type.config_description): 
119              if config1[param['name']] != config2[param['name']]: 
120                  level |= param['level'] 
121   
122          return level 
123   
124 -    def _clamp(self, config): 
125          for param in extract_params(self.type.config_description): 
126              maxval = self.type.max[param['name']] 
127              minval = self.type.min[param['name']] 
128              val = config[param['name']] 
129              if val > maxval and maxval != "": 
130                  config[param['name']] = maxval 
131              elif val < minval and minval != "": 
132                  config[param['name']] = minval 
133   
134 -    def _set_callback(self, req): 
135          return encode_config(self.update_configuration(decode_config(req.config, self.type.config_description))) 
136