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 tobe 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._exec_timeout = exec_timeout 128 self._preempt_timeout = preempt_timeout 129 self._server_wait_timeout = server_wait_timeout 130 131 # Set goal generation policy 132 if goal and hasattr(goal, '__call__'): 133 raise smach.InvalidStateError("Goal object given to SimpleActionState that IS a function object") 134 if not all([s in action_spec().action_goal.goal.__slots__ for s in goal_slots]): 135 raise smach.InvalidStateError("Goal slots specified are not valid slots.") 136 if goal_cb and not hasattr(goal_cb, '__call__'): 137 raise smach.InvalidStateError("Goal callback object given to SimpleActionState that IS NOT a function object") 138 139 # Static goal 140 if goal is None: 141 self._goal = copy.copy(action_spec().action_goal.goal) 142 else: 143 self._goal = goal 144 145 # Goal from userdata key 146 self._goal_key = goal_key 147 if goal_key is not None: 148 self.register_input_keys([goal_key]) 149 150 # Goal slots from userdata keys 151 self._goal_slots = goal_slots 152 self.register_input_keys(goal_slots) 153 154 # Goal generation callback 155 self._goal_cb = goal_cb 156 self._goal_cb_args = goal_cb_args 157 self._goal_cb_kwargs = goal_cb_kwargs 158 if smach.has_smach_interface(goal_cb): 159 self._goal_cb_input_keys = goal_cb.get_registered_input_keys() 160 self._goal_cb_output_keys = goal_cb.get_registered_output_keys() 161 162 self.register_input_keys(self._goal_cb_input_keys) 163 self.register_output_keys(self._goal_cb_output_keys) 164 else: 165 self._goal_cb_input_keys = input_keys 166 self._goal_cb_output_keys = output_keys 167 168 # Set result processing policy 169 if result_cb and not hasattr(result_cb, '__call__'): 170 raise smach.InvalidStateError("Result callback object given to SimpleActionState that IS NOT a function object") 171 if not all([s in action_spec().action_result.result.__slots__ for s in result_slots]): 172 raise smach.InvalidStateError("Result slots specified are not valid slots.") 173 174 # Result callback 175 self._result_cb = result_cb 176 self._result_cb_args = result_cb_args 177 self._result_cb_kwargs = result_cb_kwargs 178 179 if smach.has_smach_interface(result_cb): 180 self._result_cb_input_keys = result_cb.get_registered_input_keys() 181 self._result_cb_output_keys = result_cb.get_registered_output_keys() 182 self._result_cb_outcomes = result_cb.get_registered_outcomes() 183 184 self.register_input_keys(self._result_cb_input_keys) 185 self.register_output_keys(self._result_cb_output_keys) 186 self.register_outcomes(self._result_cb_outcomes) 187 else: 188 self._result_cb_input_keys = input_keys 189 self._result_cb_output_keys = output_keys 190 self._result_cb_outcomes = outcomes 191 192 # Result to userdata key 193 self._result_key = result_key 194 if result_key is not None: 195 self.register_output_keys([result_key]) 196 197 # Result slots to userdata keys 198 self._result_slots = result_slots 199 self.register_output_keys(result_slots) 200 201 # Register additional input and output keys 202 self.register_input_keys(input_keys) 203 self.register_output_keys(output_keys) 204 self.register_outcomes(outcomes) 205 206 # Declare some status variables 207 self._activate_time = rospy.Time.now() 208 self._preempt_time = rospy.Time.now() 209 self._duration = rospy.Duration(0.0) 210 self._status = SimpleActionState.WAITING_FOR_SERVER 211 212 # Construct action client, and wait for it to come active 213 self._action_client = SimpleActionClient(action_name,action_spec) 214 self._action_wait_thread = threading.Thread(name=self._action_name+'/wait_for_server',target = self._wait_for_server) 215 self._action_wait_thread.start() 216 217 self._execution_timer_thread = None 218 219 # Condition variables for threading synchronization 220 self._done_cond = threading.Condition()

221

222 - def _wait_for_server(self):

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

236

237 - def _execution_timer(self):

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

249 250 ### smach State API

251 - def request_preempt(self):

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

258

259 - def execute(self, ud):

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

388 389 ### Action client callbacks

390 - def _goal_active_cb(self):

391 """Goal Active Callback 392 This callback starts the timer that watches for the timeout specified for this action. 393 """ 394 rospy.logdebug("Action "+self._action_name+" has gone active.")

395

396 - def _goal_feedback_cb(self,feedback):

397 """Goal Feedback Callback""" 398 rospy.logdebug("Action "+self._action_name+" sent feedback.")

399

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

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

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

Source Code for Module smach_ros.simple_action_state