Source Code for Module smach_ros.service_state

  1   
  2  import roslib; roslib.load_manifest('smach_ros') 
  3  import rospy 
  4   
  5  import threading 
  6  import traceback 
  7   
  8  import smach 
  9  from smach.state import * 
 10   
 11  __all__ = ['ServiceState'] 
 12   
 13 -class ServiceState(smach.State): 
 14      """State for calling a service.""" 
 15 -    def __init__(self, 
 16              # Service info 
 17              service_name, 
 18              service_spec, 
 19              # Request Policy 
 20              request = None, 
 21              request_cb = None, 
 22              request_cb_args = [], 
 23              request_cb_kwargs = {}, 
 24              request_key = None, 
 25              request_slots = [], 
 26              # Response Policy 
 27              response_cb = None, 
 28              response_cb_args = [], 
 29              response_cb_kwargs = {}, 
 30              response_key = None, 
 31              response_slots = [], 
 32              # Keys 
 33              input_keys = [], 
 34              output_keys = [], 
 35              outcomes = [], 
 36              ): 
 37   
 38          smach.State.__init__(self,outcomes=['succeeded','aborted','preempted']) 
 39   
 40          # Store Service info 
 41          self._service_name = service_name 
 42          self._service_spec = service_spec 
 43   
 44          self._proxy = None 
 45   
 46          # Store request policy 
 47          if request is None: 
 48              self._request = service_spec._request_class() 
 49          else: 
 50              self._request = request 
 51   
 52           
 53          if request_cb is not None and not hasattr(request_cb, '__call__'): 
 54              raise smach.InvalidStateError("Request callback object given to ServiceState that IS NOT a function object") 
 55   
 56          self._request_cb = request_cb 
 57          self._request_cb_args = request_cb_args 
 58          self._request_cb_kwargs = request_cb_kwargs 
 59          if smach.has_smach_interface(request_cb): 
 60              self._request_cb_input_keys = request_cb.get_registered_input_keys() 
 61              self._request_cb_output_keys = request_cb.get_registered_output_keys() 
 62   
 63              self.register_input_keys(self._request_cb_input_keys) 
 64              self.register_output_keys(self._request_cb_output_keys) 
 65          else: 
 66              self._request_cb_input_keys = input_keys 
 67              self._request_cb_output_keys = output_keys 
 68   
 69          self._request_key = request_key 
 70          if request_key is not None: 
 71              self.register_input_keys([request_key]) 
 72   
 73          self._request_slots = request_slots 
 74          self.register_input_keys(request_slots) 
 75   
 76          # Store response policy 
 77          if response_cb is not None and not hasattr(response_cb, '__call__'): 
 78              raise smach.InvalidStateError("Response callback object given to ServiceState that IS NOT a function object") 
 79   
 80          self._response_cb = response_cb 
 81          self._response_cb_args = response_cb_args 
 82          self._response_cb_kwargs = response_cb_kwargs 
 83          if smach.has_smach_interface(response_cb): 
 84              self._response_cb_input_keys = response_cb.get_registered_input_keys() 
 85              self._response_cb_output_keys = response_cb.get_registered_output_keys() 
 86              self._response_cb_outcomes = response_cb.get_registered_outcomes() 
 87   
 88              self.register_input_keys(self._response_cb_input_keys) 
 89              self.register_output_keys(self._response_cb_output_keys) 
 90              self.register_outcomes(self._response_cb_outcomes) 
 91          else: 
 92              self._response_cb_input_keys = input_keys 
 93              self._response_cb_output_keys = output_keys 
 94              self._response_cb_outcomes = outcomes 
 95   
 96          # Register additional input and output keys 
 97          self.register_input_keys(input_keys) 
 98          self.register_output_keys(output_keys) 
 99          self.register_outcomes(outcomes) 
100   
101          self._response_key = response_key 
102          if response_key is not None: 
103              self.register_output_keys([response_key]) 
104   
105          self._response_slots = response_slots 
106          self.register_output_keys(response_slots) 
107   
108 -    def execute(self, ud): 
109          """Execute service""" 
110          # Check for preemption before executing 
111          if self.preempt_requested(): 
112              rospy.loginfo("Preempting %s before sending request." % self._service_name) 
113              self.service_preempt() 
114              return 'preempted' 
115   
116          # Make sure we're connected to the service 
117          try: 
118              while self._proxy is None: 
119                  if self.preempt_requested(): 
120                      rospy.loginfo("Preempting while waiting for service '%s'." % self._service_name) 
121                      self.service_preempt() 
122                      return 'preempted' 
123                  if rospy.is_shutdown(): 
124                      rospy.loginfo("Shutting down while waiting for service '%s'." % self._service_name) 
125                      return 'aborted' 
126                  try: 
127                      rospy.wait_for_service(self._service_name,1.0) 
128                      self._proxy = rospy.ServiceProxy(self._service_name, self._service_spec) 
129                      rospy.logdebug("Connected to service '%s'" % self._service_name) 
130                  except rospy.ROSException as ex: 
131                      rospy.logwarn("Still waiting for service '%s'..." % self._service_name) 
132          except: 
133              rospy.logwarn("Terminated while waiting for service '%s'." % self._service_name) 
134              return 'aborted' 
135   
136          # Grab request key if set 
137          if self._request_key is not None: 
138              if self._request_key in ud: 
139                  self._request = ud[self._request_key] 
140              else: 
141                  rospy.logerr("Requested request key '%s' not in userdata struture. Available keys are: %s" % (self._request_key, str(list(ud.keys())))) 
142                  return 'aborted' 
143   
144          # Write request fields from userdata if set 
145          for key in self._request_slots: 
146              if key in ud: 
147                  setattr(self._request,key,ud[key]) 
148              else: 
149                  rospy.logerr("Requested request slot key '%s' is not in userdata strcture. Available keys are: %s" % (key, str(list(ud.keys())))) 
150                  return 'aborted' 
151   
152          # Call user-supplied callback, if set, to get a request 
153          if self._request_cb is not None: 
154              try: 
155                  request_update = self._request_cb( 
156                          smach.Remapper( 
157                                  ud, 
158                                  self._request_cb_input_keys, 
159                                  self._request_cb_output_keys, 
160                                  []), 
161                          self._request, 
162                          *self._request_cb_args, 
163                          **self._request_cb_kwargs) 
164                  if request_update is not None: 
165                      self._request = request_update 
166              except: 
167                  rospy.logerr("Could not execute request callback: "+traceback.format_exc()) 
168                  return 'aborted' 
169   
170          if self._request is None: 
171              rospy.logerr("Attempting to call service "+self._service_name+" with no request") 
172              return 'aborted' 
173   
174          # Call service 
175          # Abandon hope, all ye who enter here 
176          try: 
177              rospy.logdebug("Calling service %s with request:\n%s" % (self._service_name, str(self._request))) 
178              self._response = self._proxy(self._request) 
179          except rospy.ServiceException as ex: 
180              rospy.logerr("Exception when calling service '%s': %s" % (self._service_name, str(ex))) 
181              return 'aborted' 
182   
183          # Call response callback if it's set 
184          response_cb_outcome = None 
185          if self._response_cb is not None: 
186              try: 
187                  response_cb_outcome = self._response_cb( 
188                          smach.Remapper( 
189                                  ud, 
190                                  self._response_cb_input_keys, 
191                                  self._response_cb_output_keys, 
192                                  []), 
193                          self._response, 
194                          *self._response_cb_args, 
195                          **self._response_cb_kwargs) 
196                  if response_cb_outcome is not None and response_cb_outcome not in self.get_registered_outcomes(): 
197                      rospy.logerr("Result callback for service "+self._service_name+", "+str(self._response_cb)+" was not registered with the response_cb_outcomes argument. The response callback returned '"+str(response_cb_outcome)+"' but the only registered outcomes are: "+str(self.get_registered_outcomes())) 
198                      return 'aborted' 
199              except: 
200                  rospy.logerr("Could not execute response callback: "+traceback.format_exc()) 
201                  return 'aborted' 
202   
203          if self._response_key is not None: 
204              ud[self._response_key] = self._response 
205   
206          for key in self._response_slots: 
207              ud[key] = getattr(self._response,key) 
208   
209          if response_cb_outcome is not None: 
210              return response_cb_outcome 
211   
212          return 'succeeded' 
213