controller.py
Go to the documentation of this file.
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())


rosh_experimental
Author(s): Ken Conley
autogenerated on Tue Apr 22 2014 19:45:38