smach_ros.service

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 not rospy.is_shutdown() and 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 try: 124 rospy.wait_for_service(self._service_name,1.0) 125 self._proxy = rospy.ServiceProxy(self._service_name, self._service_spec) 126 rospy.logdebug("Connected to service '%s'" % self._service_name) 127 except rospy.ROSException as ex: 128 rospy.logwarn("Still waiting for service '%s'..." % self._service_name) 129 except: 130 rospy.logwarn("Terminated while waiting for service '%s'." % self._service_name) 131 return 'aborted' 132 133 # Grab request key if set 134 if self._request_key is not None: 135 if self._request_key in ud: 136 self._request = ud[self._request_key] 137 else: 138 rospy.logerr("Requested request key '%s' not in userdata struture. Available keys are: %s" % (self._request_key, str(list(ud.keys())))) 139 return 'aborted' 140 141 # Write request fields from userdata if set 142 for key in self._request_slots: 143 if key in ud: 144 setattr(self._request,key,ud[key]) 145 else: 146 rospy.logerr("Requested request slot key '%s' is not in userdata strcture. Available keys are: %s" % (key, str(list(ud.keys())))) 147 return 'aborted' 148 149 # Call user-supplied callback, if set, to get a request 150 if self._request_cb is not None: 151 try: 152 request_update = self._request_cb( 153 smach.Remapper( 154 ud, 155 self._request_cb_input_keys, 156 self._request_cb_output_keys, 157 []), 158 self._request, 159 *self._request_cb_args, 160 **self._request_cb_kwargs) 161 if request_update is not None: 162 self._request = request_update 163 except: 164 rospy.logerr("Could not execute request callback: "+traceback.format_exc()) 165 return 'aborted' 166 167 if self._request is None: 168 rospy.logerr("Attempting to call service "+self._service_name+" with no request") 169 return 'aborted' 170 171 # Call service 172 # Abandon hope, all ye who enter here 173 try: 174 rospy.logdebug("Calling service %s with request:\n%s" % (self._service_name, str(self._request))) 175 self._response = self._proxy(self._request) 176 except rospy.ServiceException as ex: 177 rospy.logerr("Exception when calling service '%s': %s" % (self._service_name, str(ex))) 178 return 'aborted' 179 180 # Call response callback if it's set 181 response_cb_outcome = None 182 if self._response_cb is not None: 183 try: 184 response_cb_outcome = self._response_cb( 185 smach.Remapper( 186 ud, 187 self._response_cb_input_keys, 188 self._response_cb_output_keys, 189 []), 190 self._response, 191 *self._response_cb_args, 192 **self._response_cb_kwargs) 193 if response_cb_outcome is not None and response_cb_outcome not in self.get_registered_outcomes(): 194 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())) 195 return 'aborted' 196 except: 197 rospy.logerr("Could not execute response callback: "+traceback.format_exc()) 198 return 'aborted' 199 200 if self._response_key is not None: 201 ud[self._response_key] = self._response 202 203 for key in self._response_slots: 204 ud[key] = getattr(self._response,key) 205 206 if response_cb_outcome is not None: 207 return response_cb_outcome 208 209 return 'succeeded'

Source Code for Module smach_ros.service_state