controller.py

Go to the documentation of this file.

# Software License Agreement (BSD License)
#
# Copyright (c) 2010, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#  * Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#  * Redistributions in binary form must reproduce the above
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#  * Neither the name of Willow Garage, Inc. nor the names of its
#    contributors may be used to endorse or promote products derived
#    from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Revision $Id: topic.py 9987 2010-06-08 02:35:21Z kwc $

"""
Controller interfacre
"""

from __future__ import with_statement
import os
import sys
import itertools

import rostopic
import rospy

from rosh.impl.exceptions import ROSHException
from rosh.impl.namespace import Namespace, Concept

_error = None
try:
    from pr2_controller_manager import pr2_controller_manager_interface
except ImportError, e:
    _error = "Cannot load pr2_controller_manager, controllers interface is disabled"
if not _error:
    try:
        from pr2_mechanism_msgs.srv import ListControllers, SwitchController
    except ImportError, e:
        _error = "Cannot load pr2_mechanism_msgs, controllers interface is disabled"

class Controller(object):
    
    def __init__(self, name, list_service):
        self.name = name
        self._list_service = list_service

    def __repr__(self):
        try:
            resp = self._list_service()
            state = [s for c, s in itertools.izip(resp.controllers, resp.state) if c == self.name]
            if state:
                return state[0]
            else:
                return "<unknown controller>"
        except:
            import traceback
            traceback.print_exc()
            return "<unknown state>"

    def __str___(self):
        try:
            resp = self._list_service()
            state = [s for c, s in itertools.izip(resp.controllers, resp.state) if c == self.name]
            if state:
                return state[0]
            else:
                return "<unknown controller>"
        except:
            return "<unknown state>"

    def stop(self):
        pr2_controller_manager_interface.stop_controller(self.name)

    def start(self):
        pr2_controller_manager_interface.start_controller(self.name)

    # disabled because load isn't useful in the ROSH formalism (anything you can use is already loaded)
    #def load(self):
    #    pr2_controller_manager_interface.load_controller(self.name)

    def switch(self, c):
        print "disabled for now"
        if 0:
            switch_controller = rospy.ServiceProxy('pr2_controller_manager/switch_controller', SwitchController)
            start = [c]
            stop = [self.name]
            resp = s(SwitchControllerRequest(start, stop, SwitchController.STRICT))
        
    def _unload(self):
        pr2_controller_manager_interface.unload_controller(self.name)

    def kill(self):
        # based on pr2_controller_manager
        self.stop()
        self._unload()
        
    def _kill(self):
        self.kill()
        
class ControllerManager(Namespace):

    def __init__(self, name, config):
        """
        ctor.
        @param config: Namespace configuration instance with additional 'listener' attribute. 
        @type  config: L{NamespaceConfig}
        """
        super(ControllerManager, self).__init__(name, config)

        self._ns = '' # override as controllers are not namespaced
        self._controllers = {}
        self._list_service = rospy.ServiceProxy('pr2_controller_manager/list_controllers', ListControllers)

    def _get_entry(self, key):
        """
        Overrides to return non-Namespace instances at second level
        """
        cache = self._config.cache
        with self._config.lock:
            if key in cache:
                obj = cache[key]
            else:
                # create a new instance of ourselves. This requires
                # subclasses to have same constructor args.
                obj = Controller(key, self._list_service)
                cache[key] = obj
        return obj

    def _list(self):
        try:
            return self._list_service().controllers
        except:
            return []

    def _list_full(self):
        try:
            resp = self._list_service()
            return ["%s (%s)"%(c, s) for c, s in itertools.izip(resp.controllers, resp.state)]
        except:
            return []
        
    def __repr__(self):
        return self.__str__()
        
    def __str__(self):
        return '\n'.join(self._list_full())

    

class Controllers(Concept):

    def __init__(self, ctx, lock):
        super(Controllers, self).__init__(ctx, lock, ControllerManager)

    def __repr__(self):
        return self.__str__()
        
    def __str__(self):
        return '\n'.join(self._root._list_full())