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 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 hasattr(request_cb,'get_registered_input_keys')\ 60 and hasattr(request_cb,'get_registered_output_keys'): 61 self._request_cb_input_keys = request_cb.get_registered_input_keys() 62 self._request_cb_output_keys = request_cb.get_registered_output_keys() 63 64 self.register_input_keys(self._request_cb_input_keys) 65 self.register_output_keys(self._request_cb_output_keys) 66 else: 67 self._request_cb_input_keys = input_keys 68 self._request_cb_output_keys = output_keys 69 70 self._request_key = request_key 71 if request_key is not None: 72 self.register_input_keys([request_key]) 73 74 self._request_slots = request_slots 75 self.register_input_keys(request_slots) 76 77 # Store response policy 78 if response_cb and not hasattr(response_cb, '__call__'): 79 raise smach.InvalidStateError("Response callback object given to ServiceState that IS NOT a function object") 80 81 self._response_cb = response_cb 82 self._response_cb_args = response_cb_args 83 self._response_cb_kwargs = response_cb_kwargs 84 if hasattr(response_cb,'get_registered_input_keys')\ 85 and hasattr(response_cb,'get_registered_output_keys')\ 86 and hasattr(response_cb,'get_registered_outcomes'): 87 self._response_cb_input_keys = response_cb.get_registered_input_keys() 88 self._response_cb_output_keys = response_cb.get_registered_output_keys() 89 self._response_cb_outcomes = response_cb.get_registered_outcomes() 90 91 self.register_input_keys(self._response_cb_input_keys) 92 self.register_output_keys(self._response_cb_output_keys) 93 self.register_outcomes(self._response_cb_outcomes) 94 else: 95 self._response_cb_input_keys = input_keys 96 self._response_cb_output_keys = output_keys 97 self._response_cb_outcomes = outcomes 98 99 self._response_key = response_key 100 if response_key is not None: 101 self.register_output_keys([response_key]) 102 103 self._response_slots = response_slots 104 self.register_output_keys(response_slots)

105

106 - def execute(self, ud):

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

Source Code for Module smach_ros.service_state