$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: 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