smach_ros.simple_action

17 """Simple action client state. 18 19 Use this class to represent an actionlib as a state in a state machine. 20 """ 21 22 # Meta-states for this action 23 WAITING_FOR_SERVER = 0 24 INACTIVE = 1 25 ACTIVE = 2 26 PREEMPTING = 3 27 COMPLETED = 4 28

29 - def __init__(self, 30 # Action info 31 action_name, 32 action_spec, 33 # Default goal 34 goal = None, 35 goal_key = None, 36 goal_slots = [], 37 goal_cb = None, 38 goal_cb_args = [], 39 goal_cb_kwargs = {}, 40 # Result modes 41 result_key = None, 42 result_slots = [], 43 result_cb = None, 44 result_cb_args = [], 45 result_cb_kwargs = {}, 46 # Keys 47 input_keys = [], 48 output_keys = [], 49 outcomes = [], 50 # Timeouts 51 exec_timeout = None, 52 preempt_timeout = rospy.Duration(60.0), 53 server_wait_timeout = rospy.Duration(60.0) 54 ):

55 """Constructor for SimpleActionState action client wrapper. 56 57 @type action_name: string 58 @param action_name: The name of the action as it will be broadcast over ros. 59 60 @type action_spec: actionlib action msg 61 @param action_spec: The type of action to which this client will connect. 62 63 @type goal: actionlib goal msg 64 @param goal: If the goal for this action does not need to be generated at 65 runtime, it can be passed to this state on construction. 66 67 @type goal_key: string 68 @param goal_key: Pull the goal message from a key in the userdata. 69 This will be done before calling the goal_cb if goal_cb is defined. 70 71 @type goal_slots: list of string 72 @param goal_slots: Pull the goal fields (__slots__) from like-named 73 keys in userdata. This will be done before calling the goal_cb if 74 goal_cb is defined. 75 76 @type goal_cb: callable 77 @param goal_cb: If the goal for this action needs to be generated at 78 runtime, a callback can be stored which modifies the default goal 79 object. The callback is passed two parameters: 80 - userdata 81 - current stored goal 82 The callback returns a goal message. 83 84 @type result_key: string 85 @param result_key: Put the result message into the userdata with 86 the given key. This will be done after calling the result_cb 87 if result_cb is defined. 88 89 @type result_slots: list of strings 90 @param result_slots: Put the result message fields (__slots__) 91 into the userdata with keys like the field names. This will be done 92 after calling the result_cb if result_cb is defined. 93 94 @type result_cb: callable 95 @param result_cb: If result information from this action needs to be 96 stored or manipulated on reception of a result from this action, a 97 callback can be stored which is passed this information. The callback 98 is passed three parameters: 99 - userdata (L{UserData<smach.user_data.UserData>}) 100 - result status (C{actionlib.GoalStatus}) 101 - result (actionlib result msg) 102 103 @type exec_timeout: C{rospy.Duration} 104 @param exec_timeout: This is the timeout used for sending a preempt message 105 to the delegate action. This is C{None} by default, which implies no 106 timeout. 107 108 @type preempt_timeout: C{rospy.Duration} 109 @param preempt_timeout: This is the timeout used for aborting after a 110 preempt has been sent to the action and no result has been received. This 111 timeout begins counting after a preempt message has been sent. 112 113 @type server_wait_timeout: C{rospy.Duration} 114 @param server_wait_timeout: This is the timeout used for aborting while 115 waiting for an action server to become active. 116 """ 117 118 # Initialize base class 119 State.__init__(self, outcomes=['succeeded','aborted','preempted']) 120 121 # Set action properties 122 self._action_name = action_name 123 self._action_spec = action_spec 124 125 # Set timeouts 126 self._goal_status = 0 127 self._goal_result = None 128 self._exec_timeout = exec_timeout 129 self._preempt_timeout = preempt_timeout 130 self._server_wait_timeout = server_wait_timeout 131 132 # Set goal generation policy 133 if goal and hasattr(goal, '__call__'): 134 raise smach.InvalidStateError("Goal object given to SimpleActionState that IS a function object") 135 sl = action_spec().action_goal.goal.__slots__ 136 if not all([s in sl for s in goal_slots]): 137 raise smach.InvalidStateError("Goal slots specified are not valid slots. Available slots: %s; specified slots: %s" % (sl, goal_slots)) 138 if goal_cb and not hasattr(goal_cb, '__call__'): 139 raise smach.InvalidStateError("Goal callback object given to SimpleActionState that IS NOT a function object") 140 141 # Static goal 142 if goal is None: 143 self._goal = copy.copy(action_spec().action_goal.goal) 144 else: 145 self._goal = goal 146 147 # Goal from userdata key 148 self._goal_key = goal_key 149 if goal_key is not None: 150 self.register_input_keys([goal_key]) 151 152 # Goal slots from userdata keys 153 self._goal_slots = goal_slots 154 self.register_input_keys(goal_slots) 155 156 # Goal generation callback 157 self._goal_cb = goal_cb 158 self._goal_cb_args = goal_cb_args 159 self._goal_cb_kwargs = goal_cb_kwargs 160 if smach.has_smach_interface(goal_cb): 161 self._goal_cb_input_keys = goal_cb.get_registered_input_keys() 162 self._goal_cb_output_keys = goal_cb.get_registered_output_keys() 163 164 self.register_input_keys(self._goal_cb_input_keys) 165 self.register_output_keys(self._goal_cb_output_keys) 166 else: 167 self._goal_cb_input_keys = input_keys 168 self._goal_cb_output_keys = output_keys 169 170 # Set result processing policy 171 if result_cb and not hasattr(result_cb, '__call__'): 172 raise smach.InvalidStateError("Result callback object given to SimpleActionState that IS NOT a function object") 173 if not all([s in action_spec().action_result.result.__slots__ for s in result_slots]): 174 raise smach.InvalidStateError("Result slots specified are not valid slots.") 175 176 # Result callback 177 self._result_cb = result_cb 178 self._result_cb_args = result_cb_args 179 self._result_cb_kwargs = result_cb_kwargs 180 181 if smach.has_smach_interface(result_cb): 182 self._result_cb_input_keys = result_cb.get_registered_input_keys() 183 self._result_cb_output_keys = result_cb.get_registered_output_keys() 184 self._result_cb_outcomes = result_cb.get_registered_outcomes() 185 186 self.register_input_keys(self._result_cb_input_keys) 187 self.register_output_keys(self._result_cb_output_keys) 188 self.register_outcomes(self._result_cb_outcomes) 189 else: 190 self._result_cb_input_keys = input_keys 191 self._result_cb_output_keys = output_keys 192 self._result_cb_outcomes = outcomes 193 194 # Result to userdata key 195 self._result_key = result_key 196 if result_key is not None: 197 self.register_output_keys([result_key]) 198 199 # Result slots to userdata keys 200 self._result_slots = result_slots 201 self.register_output_keys(result_slots) 202 203 # Register additional input and output keys 204 self.register_input_keys(input_keys) 205 self.register_output_keys(output_keys) 206 self.register_outcomes(outcomes) 207 208 # Declare some status variables 209 self._activate_time = rospy.Time.now() 210 self._preempt_time = rospy.Time.now() 211 self._duration = rospy.Duration(0.0) 212 self._status = SimpleActionState.WAITING_FOR_SERVER 213 214 # Construct action client, and wait for it to come active 215 self._action_client = SimpleActionClient(action_name, action_spec) 216 self._action_wait_thread = threading.Thread(name=self._action_name+'/wait_for_server', target=self._wait_for_server) 217 self._action_wait_thread.start() 218 219 self._execution_timer_thread = None 220 221 # Condition variables for threading synchronization 222 self._done_cond = threading.Condition()

223

224 - def _wait_for_server(self):

225 """Internal method for waiting for the action server 226 This is run in a separate thread and allows construction of this state 227 to not block the construction of other states. 228 """ 229 while self._status == SimpleActionState.WAITING_FOR_SERVER and not rospy.is_shutdown(): 230 try: 231 if self._action_client.wait_for_server(rospy.Duration(1.0)):#self._server_wait_timeout): 232 self._status = SimpleActionState.INACTIVE 233 if self.preempt_requested(): 234 return 235 except: 236 if not rospy.core._in_shutdown: # This is a hack, wait_for_server should not throw an exception just because shutdown was called 237 rospy.logerr("Failed to wait for action server '%s'" % (self._action_name))

238

239 - def _execution_timer(self):

240 """Internal method for cancelling a timed out goal after a timeout.""" 241 while self._status == SimpleActionState.ACTIVE and not rospy.is_shutdown(): 242 try: 243 rospy.sleep(0.1) 244 except: 245 if not rospy.is_shutdown(): 246 rospy.logerr("Failed to sleep while running '%s'" % self._action_name) 247 if rospy.Time.now() - self._activate_time > self._exec_timeout: 248 rospy.logwarn("Action %s timed out after %d seconds." % (self._action_name, self._exec_timeout.to_sec())) 249 # Cancel the goal 250 self._action_client.cancel_goal()

251 252 ### smach State API

253 - def request_preempt(self):

254 rospy.loginfo("Preempt requested on action '%s'" % (self._action_name)) 255 smach.State.request_preempt(self) 256 if self._status == SimpleActionState.ACTIVE: 257 rospy.loginfo("Preempt on action '%s' cancelling goal: \n%s" % (self._action_name, str(self._goal))) 258 # Cancel the goal 259 self._action_client.cancel_goal()

260

261 - def execute(self, ud):

262 """Called when executing a state. 263 This calls the goal_cb if it is defined, and then dispatches the 264 goal with a non-blocking call to the action client. 265 """ 266 267 # Make sure we're connected to the action server 268 if self._status is SimpleActionState.WAITING_FOR_SERVER: 269 rospy.logwarn("Still waiting for action server '%s' to start... is it running?" % self._action_name) 270 if self._action_wait_thread.is_alive(): 271 # Block on joining the server wait thread (This can be preempted) 272 self._action_wait_thread.join() 273 else: 274 # Wait for the server in this thread (This can also be preempted) 275 self._wait_for_server() 276 277 if not self.preempt_requested(): 278 # In case of preemption we probably didn't connect 279 rospy.loginfo("Connected to action server '%s'." % self._action_name) 280 281 # Check for preemption before executing 282 if self.preempt_requested(): 283 rospy.loginfo("Preempting %s before sending goal." % self._action_name) 284 self.service_preempt() 285 return 'preempted' 286 287 # We should only get here if we have connected to the server 288 if self._status is SimpleActionState.WAITING_FOR_SERVER: 289 rospy.logfatal("Action server for "+self._action_name+" is not running.") 290 return 'aborted' 291 else: 292 self._status = SimpleActionState.INACTIVE 293 294 # Grab goal key, if set 295 if self._goal_key is not None: 296 self._goal = ud[self._goal_key] 297 298 # Write goal fields from userdata if set 299 for key in self._goal_slots: 300 setattr(self._goal, key, ud[key]) 301 302 # Call user-supplied callback, if set, to get a goal 303 if self._goal_cb is not None: 304 try: 305 goal_update = self._goal_cb( 306 smach.Remapper( 307 ud, 308 self._goal_cb_input_keys, 309 self._goal_cb_output_keys, 310 []), 311 self._goal, 312 *self._goal_cb_args, 313 **self._goal_cb_kwargs) 314 if goal_update is not None: 315 self._goal = goal_update 316 except: 317 rospy.logerr("Could not execute goal callback: "+traceback.format_exc()) 318 return 'aborted' 319 320 # Make sure the necessary paramters have been set 321 if self._goal is None and self._goal_cb is None: 322 rospy.logerr("Attempting to activate action "+self._action_name+" with no goal or goal callback set. Did you construct the SimpleActionState properly?") 323 return 'aborted' 324 325 # Dispatch goal via non-blocking call to action client 326 self._activate_time = rospy.Time.now() 327 self._status = SimpleActionState.ACTIVE 328 329 # Wait on done condition 330 self._done_cond.acquire() 331 self._action_client.send_goal(self._goal, self._goal_done_cb, self._goal_active_cb, self._goal_feedback_cb) 332 333 # Preempt timeout watch thread 334 if self._exec_timeout: 335 self._execution_timer_thread = threading.Thread(name=self._action_name+'/preempt_watchdog', target=self._execution_timer) 336 self._execution_timer_thread.start() 337 338 # Wait for action to finish 339 self._done_cond.wait() 340 341 # Call user result callback if defined 342 result_cb_outcome = None 343 if self._result_cb is not None: 344 try: 345 result_cb_outcome = self._result_cb( 346 smach.Remapper( 347 ud, 348 self._result_cb_input_keys, 349 self._result_cb_output_keys, 350 []), 351 self._goal_status, 352 self._goal_result) 353 if result_cb_outcome is not None and result_cb_outcome not in self.get_registered_outcomes(): 354 rospy.logerr("Result callback for action "+self._action_name+", "+str(self._result_cb)+" was not registered with the result_cb_outcomes argument. The result callback returned '"+str(result_cb_outcome)+"' but the only registered outcomes are: "+str(self.get_registered_outcomes())) 355 return 'aborted' 356 except: 357 rospy.logerr("Could not execute result callback: "+traceback.format_exc()) 358 return 'aborted' 359 360 if self._result_key is not None: 361 ud[self._result_key] = self._goal_result 362 363 for key in self._result_slots: 364 ud[key] = getattr(self._goal_result, key) 365 366 # Check status 367 if self._status == SimpleActionState.INACTIVE: 368 # Set the outcome on the result state 369 if self._goal_status == GoalStatus.SUCCEEDED: 370 outcome = 'succeeded' 371 elif self._goal_status == GoalStatus.PREEMPTED and self.preempt_requested(): 372 outcome = 'preempted' 373 self.service_preempt() 374 else: 375 # All failures at this level are captured by aborting, even if we timed out 376 # This is an important distinction between local preemption, and preemption 377 # from above. 378 outcome = 'aborted' 379 else: 380 # We terminated without going inactive 381 rospy.logwarn("Action state terminated without going inactive first.") 382 outcome = 'aborted' 383 384 # Check custom result cb outcome 385 if result_cb_outcome is not None: 386 outcome = result_cb_outcome 387 388 # Set status inactive 389 self._status = SimpleActionState.INACTIVE 390 self._done_cond.release() 391 392 return outcome

393 394 ### Action client callbacks

395 - def _goal_active_cb(self):

396 """Goal Active Callback 397 This callback starts the timer that watches for the timeout specified for this action. 398 """ 399 rospy.logdebug("Action "+self._action_name+" has gone active.")

400

401 - def _goal_feedback_cb(self, feedback):

402 """Goal Feedback Callback""" 403 rospy.logdebug("Action "+self._action_name+" sent feedback.")

404

405 - def _goal_done_cb(self, result_state, result):

406 """Goal Done Callback 407 This callback resets the active flags and reports the duration of the action. 408 Also, if the user has defined a result_cb, it is called here before the 409 method returns. 410 """ 411 def get_result_str(i): 412 strs = ('PENDING','ACTIVE','PREEMPTED','SUCCEEDED','ABORTED','REJECTED','LOST') 413 if i < len(strs): 414 return strs[i] 415 else: 416 return 'UNKNOWN ('+str(i)+')'

417 418 # Calculate duration 419 self._duration = rospy.Time.now() - self._activate_time 420 rospy.logdebug("Action "+self._action_name+" terminated after "\ 421 +str(self._duration.to_sec())+" seconds with result "\ 422 +get_result_str(self._action_client.get_state())+".") 423 424 # Store goal state 425 self._goal_status = result_state 426 self._goal_result = result 427 428 # Rest status 429 self._status = SimpleActionState.INACTIVE 430 431 # Notify done 432 self._done_cond.acquire() 433 self._done_cond.notify() 434 self._done_cond.release()

Source Code for Module smach_ros.simple_action_state