smach.concurrence

11 """Concurrence Container 12 13 This state allows for simple split-join concurrency. The user adds a set of 14 states which are all executed simultaneously. The concurrent split state 15 can only transition once all conatained states are ready to transition. 16 17 This container can be configured to return a given outcome as a function of 18 the outcomes of the contained states. This is specified in the constructor 19 of the class, or after construction with L{Concurrence.add_outcome_map}. 20 21 While a concurrence will not terminate until all if its children terminate, 22 it is possible for it to preempt a subset of states 23 - All child states terminate 24 - At least one child state terminates 25 - A user-defined callback signals termination 26 27 Given these causes of termination, the outcome can be determined in four ways: 28 - A user-defined callback returns an outcome 29 - A child-outcome map which requires ALL states to terminate is satisfied 30 - A child-outcome map which requires ONE state to terminate is satisfied 31 - No maps are satisfied, so the default outcome is returned 32 33 The specification of the outcome maps and the outcome callback are 34 described in the constructor documentation below. More than one policy can 35 be supplied, and each policy has the potential to not be satisfied. In the 36 situation in which multiple policies are provided, and a given policy is 37 not satisfied, the outcome choice precedence is as follows: 38 - Outcome callback 39 - First-triggered outcome map 40 - last-triggered outcome map 41 - Default outcome 42 43 In practive it is best to try to accomplish your task with just ONE outcome 44 policy. 45 46 """

47 - def __init__(self, 48 outcomes, 49 default_outcome, 50 input_keys = [], 51 output_keys = [], 52 outcome_map = {}, 53 outcome_cb = None, 54 child_termination_cb = None 55 ):

56 """Constructor for smach Concurrent Split. 57 58 @type outcomes: list of strings 59 @param outcomes: The potential outcomes of this state machine. 60 61 @type default_outcome: string 62 @param default_outcome: The outcome of this state if no elements in the 63 outcome map are satisfied by the outcomes of the contained states. 64 65 66 @type outcome_map: list 67 @param outcome_map: This is an outcome map for determining the 68 outcome of this container. Each outcome of the container is mapped 69 to a dictionary mapping child labels onto outcomes. If none of the 70 child-outcome maps is satisfied, the concurrence will terminate 71 with thhe default outcome. 72 73 For example, if the and_outcome_map is: 74 {'succeeded' : {'FOO':'succeeded', 'BAR':'done'}, 75 'aborted' : {'FOO':'aborted'}} 76 Then the concurrence will terimate with outcome 'succeeded' only if 77 BOTH states 'FOO' and 'BAR' have terminated 78 with outcomes 'succeeded' and 'done', respectively. The outcome 79 'aborted' will be returned by the concurrence if the state 'FOO' 80 returns the outcome 'aborted'. 81 82 If the outcome of a state is not specified, it will be treated as 83 irrelevant to the outcome of the concurrence 84 85 If the criteria for one outcome is the subset of another outcome, 86 the container will choose the outcome which has more child outcome 87 criteria satisfied. If both container outcomes have the same 88 number of satisfied criteria, the behavior is undefined. 89 90 If a more complex outcome policy is required, see the user can 91 provide an outcome callback. See outcome_cb, below. 92 93 @type child_termination_cb: callale 94 @param child_termination_cb: This callback gives the user the ability 95 to force the concurrence to preempt running states given the 96 termination of some other set of states. This is useful when using 97 a concurrence as a monitor container. 98 99 This callback is called each time a child state terminates. It is 100 passed a single argument, a dictionary mapping child state labels 101 onto their outcomes. If a state has not yet terminated, it's dict 102 value will be None. 103 104 This function can return three things: 105 - False: continue blocking on the termination of all other states 106 - True: Preempt all other states 107 - list of state labels: Preempt only the specified states 108 109 I{If you just want the first termination to cause the other children 110 to terminate, the callback (lamda so: True) will always return True.} 111 112 @type outcome_cb: callable 113 @param outcome_cb: If the outcome policy needs to be more complicated 114 than just a conjunction of state outcomes, the user can supply 115 a callback for specifying the outcome of the container. 116 117 This callback is called only once all child states have terminated, 118 and it is passed the dictionary mapping state labels onto their 119 respective outcomes. 120 121 If the callback returns a string, it will treated as the outcome of 122 the container. 123 124 If the callback returns None, the concurrence will first check the 125 outcome_map, and if no outcome in the outcome_map is satisfied, it 126 will return the default outcome. 127 128 B{NOTE: This callback should be a function ONLY of the outcomes of 129 the child states. It should not access any other resources.} 130 131 """ 132 smach.container.Container.__init__(self, outcomes, input_keys, output_keys) 133 134 # List of concurrent states 135 self._states = {} 136 self._threads = {} 137 self._remappings = {} 138 139 if not (default_outcome or outcome_map or outcome_cb): 140 raise smach.InvalidStateError("Concurrence requires an outcome policy") 141 142 # Initialize error string 143 errors = "" 144 145 # Check if default outcome is necessary 146 if default_outcome != str(default_outcome): 147 errors += "\n\tDefault outcome '%s' does not appear to be a string." % str(default_outcome) 148 if default_outcome not in outcomes: 149 errors += "\n\tDefault outcome '%s' is unregistered." % str(default_outcome) 150 151 # Check if outcome maps only contain outcomes that are registered 152 for o in outcome_map: 153 if o not in outcomes: 154 errors += "\n\tUnregistered outcome '%s' in and_outcome_map." % str(o) 155 156 # Check if outcome cb is callable 157 if outcome_cb and not hasattr(outcome_cb,'__call__'): 158 errors += "\n\tOutcome callback '%s' is not callable." % str(outcome_cb) 159 160 # Check if child termination cb is callable 161 if child_termination_cb and not hasattr(child_termination_cb,'__call__'): 162 errors += "\n\tChild termination callback '%s' is not callable." % str(child_termination_cb) 163 164 # Report errors 165 if len(errors) > 0: 166 raise smach.InvalidStateError("Errors specifying outcome policy of concurrence: %s" % errors) 167 168 # Store outcome policies 169 self._default_outcome = default_outcome 170 self._outcome_map = outcome_map 171 self._outcome_cb = outcome_cb 172 self._child_termination_cb = child_termination_cb 173 self._child_outcomes = {} 174 175 # Condition variables for threading synchronization 176 self._user_code_exception = False 177 self._done_cond = threading.Condition() 178 self._ready_event = threading.Event()

179 180 ### Construction methods 181 @staticmethod

182 - def add(label, state, remapping={}):

183 """Add state to the opened concurrence. 184 This state will need to terminate before the concurrence terminates. 185 """ 186 # Get currently opened container 187 self = Concurrence._currently_opened_container() 188 189 # Store state 190 self._states[label] = state 191 self._remappings[label] = remapping 192 193 return state

194 195 ### State interface

196 - def execute(self, parent_ud = smach.UserData()):

197 """Overridden execute method. 198 This starts all the threads. 199 """ 200 # Clear the ready event 201 self._ready_event.clear() 202 203 # Reset child outcomes 204 self._child_outcomes = {} 205 206 # Copy input keys 207 self._copy_input_keys(parent_ud, self.userdata) 208 209 # Spew some info 210 smach.loginfo("Concurrence starting with userdata: \n\t%s" % 211 (str(list(self.userdata.keys())))) 212 213 # Call start callbacks 214 self.call_start_cbs() 215 216 # Create all the threads 217 for (label, state) in ((k,self._states[k]) for k in self._states): 218 # Initialize child outcomes 219 self._child_outcomes[label] = None 220 self._threads[label] = threading.Thread( 221 name='concurrent_split:'+label, 222 target=self._state_runner, 223 args=(label,)) 224 225 # Launch threads 226 for thread in self._threads.values(): 227 thread.start() 228 229 # Wait for done notification 230 self._done_cond.acquire() 231 232 # Notify all threads ready to go 233 self._ready_event.set() 234 235 # Wait for a done notification from a thread 236 self._done_cond.wait() 237 self._done_cond.release() 238 239 # Preempt any running states 240 smach.logdebug("SMACH Concurrence preempting running states.") 241 for label in self._states: 242 if self._child_outcomes[label] == None: 243 self._states[label].request_preempt() 244 245 # Wait for all states to terminate 246 while not smach.is_shutdown(): 247 if all([not t.isAlive() for t in self._threads.values()]): 248 break 249 self._done_cond.acquire() 250 self._done_cond.wait(0.1) 251 self._done_cond.release() 252 253 # Check for user code exception 254 if self._user_code_exception: 255 self._user_code_exception = False 256 raise smach.InvalidStateError("A concurrent state raised an exception during execution.") 257 258 # Check for preempt 259 if self.preempt_requested(): 260 # initialized serviced flag 261 children_preempts_serviced = True 262 263 # Service this preempt if 264 for (label,state) in ((k,self._states[k]) for k in self._states): 265 if state.preempt_requested(): 266 # Reset the flag 267 children_preempts_serviced = False 268 # Complain 269 smach.logwarn("State '%s' in concurrence did not service preempt." % label) 270 # Recall the preempt if it hasn't been serviced 271 state.recall_preempt() 272 if children_preempts_serviced: 273 smach.loginfo("Concurrence serviced preempt.") 274 self.service_preempt() 275 276 # Spew some debyg info 277 smach.loginfo("Concurrent Outcomes: "+str(self._child_outcomes)) 278 279 # Initialize the outcome 280 outcome = self._default_outcome 281 282 # Determine the outcome from the outcome map 283 smach.logdebug("SMACH Concurrence determining contained state outcomes.") 284 for (container_outcome, outcomes) in ((k,self._outcome_map[k]) for k in self._outcome_map): 285 if all([self._child_outcomes[label] == outcomes[label] for label in outcomes]): 286 smach.logdebug("Terminating concurrent split with mapped outcome.") 287 outcome = container_outcome 288 289 # Check outcome callback 290 if self._outcome_cb: 291 try: 292 cb_outcome = self._outcome_cb(copy.copy(self._child_outcomes)) 293 if cb_outcome: 294 if cb_outcome == str(cb_outcome): 295 outcome = cb_outcome 296 else: 297 smach.logerr("Outcome callback returned a non-string '%s', using default outcome '%s'" % (str(cb_outcome), self._default_outcome)) 298 else: 299 smach.logwarn("Outcome callback returned None, using outcome '%s'" % outcome) 300 except: 301 raise smach.InvalidUserCodeError(("Could not execute outcome callback '%s': " % self._outcome_cb)+traceback.format_exc()) 302 303 # Cleanup 304 self._threads = {} 305 self._child_outcomes = {} 306 307 # Call termination callbacks 308 self.call_termination_cbs(list(self._states.keys()), outcome) 309 310 # Copy output keys 311 self._copy_output_keys(self.userdata, parent_ud) 312 313 return outcome

314

315 - def request_preempt(self):

316 """Preempt all contained states.""" 317 # Set preempt flag 318 smach.State.request_preempt(self) 319 320 # Notify concurrence that it should preempt running states and terminate 321 with self._done_cond: 322 self._done_cond.notify_all()

323 324

325 - def _state_runner(self,label):

326 """Runs the states in parallel threads.""" 327 328 # Wait until all threads are ready to start before beginnging 329 self._ready_event.wait() 330 331 self.call_transition_cbs() 332 333 # Execute child state 334 try: 335 self._child_outcomes[label] = self._states[label].execute(smach.Remapper( 336 self.userdata, 337 self._states[label].get_registered_input_keys(), 338 self._states[label].get_registered_output_keys(), 339 self._remappings[label])) 340 except: 341 self._user_code_exception = True 342 with self._done_cond: 343 self._done_cond.notify_all() 344 raise smach.InvalidStateError(("Could not execute child state '%s': " % label)+traceback.format_exc()) 345 346 # Make sure the child returned an outcome 347 if self._child_outcomes[label] is None: 348 raise smach.InvalidStateError("Concurrent state '%s' returned no outcome on termination." % label) 349 else: 350 smach.loginfo("Concurrent state '%s' returned outcome '%s' on termination." % (label, self._child_outcomes[label])) 351 352 # Check if all of the states have completed 353 with self._done_cond: 354 # initialize preemption flag 355 preempt_others = False 356 # Call transition cb's 357 self.call_transition_cbs() 358 # Call child termination cb if it's defined 359 if self._child_termination_cb: 360 try: 361 preempt_others = self._child_termination_cb(self._child_outcomes) 362 except: 363 raise smach.InvalidUserCodeError("Could not execute child termination callback: "+traceback.format_exc()) 364 365 # Notify the container to terminate (and preempt other states if neceesary) 366 if preempt_others or all([o is not None for o in self._child_outcomes.values()]): 367 self._done_cond.notify_all()

368 369 ### Container interface

370 - def get_children(self):

371 return self._states

372

373 - def __getitem__(self,key):

374 return self._states[key]

375

376 - def get_initial_states(self):

377 return list(self._states.keys())

378

379 - def set_initial_state(self, initial_states, userdata):

380 if initial_states > 0: 381 if initial_states < len(self._states): 382 smach.logwarn("Attempting to set initial states in Concurrence" 383 " container, but Concurrence children are always" 384 " all executed initially, ignoring call.") 385 386 # Set local userdata 387 self.userdata.update(userdata)

388

389 - def get_active_states(self):

390 return [label for (label,outcome) in ((k,self._child_outcomes[k]) for k in self._child_outcomes) if outcome is None]

391

392 - def get_internal_edges(self):

393 int_edges = [] 394 for (container_outcome, outcomes) in ((k,self._outcome_map[k]) for k in self._outcome_map): 395 for state_key in outcomes: 396 int_edges.append((outcomes[state_key], state_key, container_outcome)) 397 return int_edges

398

399 - def check_consistency(self):

400 for (co,cso) in ((k,self._outcome_map[k]) for k in self._outcome_map): 401 for state_label,outcome in ((k,cso[k]) for k in cso): 402 if outcome not in self._states[state_label].get_registered_outcomes(): 403 raise smach.InvalidTransitionError( 404 'Outcome map in SMACH Concurrence references a state outcome that does not exist. Requested state outcome: \'%s\', but state \'%s\' only has outcomes %s' % 405 (outcome, state_label, str(self._states[state_label].get_registered_outcomes())))

Source Code for Module smach.concurrence