action.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: action.py 11460 2010-10-08 05:21:05Z kwc $
00034 
00035 from __future__ import with_statement
00036 
00037 import time
00038 
00039 import roslib.packages
00040 import rosmsg
00041 import rospy
00042 
00043 import actionlib
00044 import rostopic
00045 
00046 from rosh.impl.namespace import Namespace, Concept
00047 import rosh.impl.msg
00048 import rosh.impl.topic
00049     
00050 class Container(object):
00051     def __init__(self):
00052         self.obj = None
00053     def __call__(self, arg):
00054         self.obj = arg
00055         
00056 #TODO: wait for result?
00057 class Goal(object):
00058     
00059     def __init__(self, topic, goal_id):
00060         self.topic = topic
00061         self.goal_id = goal_id
00062     
00063     def __repr__(self):
00064         return self.goal_id.id
00065     
00066     #def result(self):
00067     #    return self.topic.result()
00068         
00069     def cancel(self):
00070         # Contract of actionlib is id kills id, adding a stamp kills
00071         # all goals prior to stamp.
00072         self.topic.cancel(id=self.goal_id.id)
00073     
00074 class Action(Namespace):
00075 
00076     def __init__(self, name, config):
00077         """
00078         ctor.
00079         @param config: Namespace configuration instance with additional 'listener' attribute. 
00080         @type  config: L{NamespaceConfig}
00081         """
00082         super(Action, self).__init__(name, config)
00083         self._type = self._type_name = None
00084         self._goal = None
00085 
00086         self._init_action()
00087 
00088     def _get_type(self):
00089         "rostype() API"
00090         return self._type
00091 
00092     def status(self):
00093         base_topic = self._config.ctx.topics[self._name]
00094         status_topic = base_topic['status']
00095         # this is an unstable API
00096         c = Container()
00097         with rosh.impl.topic.subscribe(status_topic, c):
00098             while c.obj is None:
00099                 time.sleep(0.001)
00100         return c.obj
00101     
00102     def goals(self):
00103         s = self.status()
00104         # type is actionlib_msgs/GoalStatusArray
00105         base_topic = self._config.ctx.topics[self._name]
00106         return [Goal(base_topic, s.goal_id) for s in s.status_list]
00107 
00108     def __getitem__(self, key):
00109         if key in ['goals', 'status']:
00110             return object.__getattribute__(self, key)
00111         else:
00112             return super(Action, self).__getitem__(key)
00113         
00114     def _list(self):
00115         """
00116         Override Namespace._list()
00117         """
00118         try:
00119             # feedback may become optional in the future, so we don't
00120             # match against it
00121 
00122             #TODO: setup caching policy for getPublishedTopics
00123             pubs = self._config.master.getPublishedTopics(self._ns)
00124             candidates = [p[0] for p in pubs if p[0].endswith('/status')]
00125             pub_names = [p[0] for p in pubs]
00126             matches = []
00127             for c in candidates:
00128                 stem = c[:-len('status')]
00129                 if stem and stem+'result' in pub_names:
00130                     matches.append(stem)
00131             if self._name and matches:
00132                 return matches + ['/goals', '/status']
00133             else:
00134                 return matches
00135         except:
00136             return []
00137 
00138     def __repr__(self):
00139         return self.__str__()
00140         
00141     def __str__(self):
00142         # re-initialize
00143         if self._type is None and self._type_name is not None:
00144             self._init_action()
00145             
00146         if self._type is not None:        
00147             return rosmsg.get_msg_text(self._goal._type)
00148         else:
00149             return self._ns
00150 
00151     def _init_action(self):
00152         if self._ns == '/':
00153             return
00154         if self._type is None:
00155             #TODO: shift away from feedback, use status instead
00156             feedback_topic = roslib.names.ns_join(self._ns, 'feedback')
00157             feedback_type_name = rostopic.get_topic_type(feedback_topic, blocking=False)[0]
00158             if feedback_type_name and feedback_type_name.endswith('Feedback'):
00159                 self._type_name = feedback_type_name[:-len('Feedback')]
00160                 if not self._type_name.endswith('Action'):
00161                     # abort init
00162                     self._type_name = None
00163                 else:
00164                     # rosh's loader has extra toys
00165                     self._type = rosh.impl.msg.get_message_class(self._type_name)
00166                     if self._type is not None:
00167                         # only load if the previous load succeeded
00168                         self._goal = rosh.impl.msg.get_message_class(self._type_name[:-len('Action')]+'Goal')
00169 
00170     def __call__(self, *args, **kwds):
00171         """
00172         @return: current transform
00173         @rtype: L{Transform}
00174         """
00175         if self._type is None:
00176             self._init_action()
00177         goal = self._goal(*args, **kwds)
00178         client = actionlib.SimpleActionClient(self._name[1:], self._type)
00179         client.wait_for_server()
00180         client.send_goal(goal)
00181         client.wait_for_result()
00182         return client.get_result()
00183 
00184 class Actions(Concept):
00185 
00186     def __init__(self, ctx, lock):
00187         super(Actions, self).__init__(ctx, lock, Action)
00188 


rosh_common
Author(s): Ken Conley
autogenerated on Fri Aug 28 2015 12:40:17