$search
00001 # Software License Agreement (BSD License) 00002 # 00003 # Copyright (c) 2010, 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 # Revision $Id: topic.py 9987 2010-06-08 02:35:21Z kwc $ 00034 00035 """ 00036 Controller interfacre 00037 """ 00038 00039 from __future__ import with_statement 00040 import os 00041 import sys 00042 import itertools 00043 00044 import rostopic 00045 import rospy 00046 00047 from rosh.impl.exceptions import ROSHException 00048 from rosh.impl.namespace import Namespace, Concept 00049 00050 _error = None 00051 try: 00052 from pr2_controller_manager import pr2_controller_manager_interface 00053 except ImportError, e: 00054 _error = "Cannot load pr2_controller_manager, controllers interface is disabled" 00055 if not _error: 00056 try: 00057 from pr2_mechanism_msgs.srv import ListControllers, SwitchController 00058 except ImportError, e: 00059 _error = "Cannot load pr2_mechanism_msgs, controllers interface is disabled" 00060 00061 class Controller(object): 00062 00063 def __init__(self, name, list_service): 00064 self.name = name 00065 self._list_service = list_service 00066 00067 def __repr__(self): 00068 try: 00069 resp = self._list_service() 00070 state = [s for c, s in itertools.izip(resp.controllers, resp.state) if c == self.name] 00071 if state: 00072 return state[0] 00073 else: 00074 return "<unknown controller>" 00075 except: 00076 import traceback 00077 traceback.print_exc() 00078 return "<unknown state>" 00079 00080 def __str___(self): 00081 try: 00082 resp = self._list_service() 00083 state = [s for c, s in itertools.izip(resp.controllers, resp.state) if c == self.name] 00084 if state: 00085 return state[0] 00086 else: 00087 return "<unknown controller>" 00088 except: 00089 return "<unknown state>" 00090 00091 def stop(self): 00092 pr2_controller_manager_interface.stop_controller(self.name) 00093 00094 def start(self): 00095 pr2_controller_manager_interface.start_controller(self.name) 00096 00097 # disabled because load isn't useful in the ROSH formalism (anything you can use is already loaded) 00098 #def load(self): 00099 # pr2_controller_manager_interface.load_controller(self.name) 00100 00101 def switch(self, c): 00102 print "disabled for now" 00103 if 0: 00104 switch_controller = rospy.ServiceProxy('pr2_controller_manager/switch_controller', SwitchController) 00105 start = [c] 00106 stop = [self.name] 00107 resp = s(SwitchControllerRequest(start, stop, SwitchController.STRICT)) 00108 00109 def _unload(self): 00110 pr2_controller_manager_interface.unload_controller(self.name) 00111 00112 def kill(self): 00113 # based on pr2_controller_manager 00114 self.stop() 00115 self._unload() 00116 00117 def _kill(self): 00118 self.kill() 00119 00120 class ControllerManager(Namespace): 00121 00122 def __init__(self, name, config): 00123 """ 00124 ctor. 00125 @param config: Namespace configuration instance with additional 'listener' attribute. 00126 @type config: L{NamespaceConfig} 00127 """ 00128 super(ControllerManager, self).__init__(name, config) 00129 00130 self._ns = '' # override as controllers are not namespaced 00131 self._controllers = {} 00132 self._list_service = rospy.ServiceProxy('pr2_controller_manager/list_controllers', ListControllers) 00133 00134 def _get_entry(self, key): 00135 """ 00136 Overrides to return non-Namespace instances at second level 00137 """ 00138 cache = self._config.cache 00139 with self._config.lock: 00140 if key in cache: 00141 obj = cache[key] 00142 else: 00143 # create a new instance of ourselves. This requires 00144 # subclasses to have same constructor args. 00145 obj = Controller(key, self._list_service) 00146 cache[key] = obj 00147 return obj 00148 00149 def _list(self): 00150 try: 00151 return self._list_service().controllers 00152 except: 00153 return [] 00154 00155 def _list_full(self): 00156 try: 00157 resp = self._list_service() 00158 return ["%s (%s)"%(c, s) for c, s in itertools.izip(resp.controllers, resp.state)] 00159 except: 00160 return [] 00161 00162 def __repr__(self): 00163 return self.__str__() 00164 00165 def __str__(self): 00166 return '\n'.join(self._list_full()) 00167 00168 00169 00170 class Controllers(Concept): 00171 00172 def __init__(self, ctx, lock): 00173 super(Controllers, self).__init__(ctx, lock, ControllerManager) 00174 00175 def __repr__(self): 00176 return self.__str__() 00177 00178 def __str__(self): 00179 return '\n'.join(self._root._list_full())