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 timeout_time = rospy.get_rostime() + self._server_wait_timeout 230 while self._status == SimpleActionState.WAITING_FOR_SERVER and not rospy.is_shutdown() and not rospy.get_rostime() >= timeout_time: 231 try: 232 if self._action_client.wait_for_server(rospy.Duration(1.0)):#self._server_wait_timeout): 233 self._status = SimpleActionState.INACTIVE 234 if self.preempt_requested(): 235 return 236 except: 237 if not rospy.core._in_shutdown: # This is a hack, wait_for_server should not throw an exception just because shutdown was called 238 rospy.logerr("Failed to wait for action server '%s'" % (self._action_name))

239

240 - def _execution_timer(self):

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

252 253 ### smach State API

254 - def request_preempt(self):

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

261

262 - def execute(self, ud):

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

394 395 ### Action client callbacks

396 - def _goal_active_cb(self):

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

401

402 - def _goal_feedback_cb(self, feedback):

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

405

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

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

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

Source Code for Module smach_ros.simple_action_state