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 179 ### Construction methods 180 @staticmethod

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

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

193 194 ### State interface

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

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

305

306 - def request_preempt(self):

307 """Preempt all contained states.""" 308 # Set preempt flag 309 smach.State.request_preempt(self) 310 311 # Notify concurrence that it should preempt running states and terminate 312 with self._done_cond: 313 self._done_cond.notify_all()

314 315

316 - def _state_runner(self,label):

317 """Runs the states in parallel threads.""" 318 self.call_transition_cbs() 319 320 # Execute child state 321 try: 322 self._child_outcomes[label] = self._states[label].execute(smach.Remapper( 323 self.userdata, 324 self._states[label].get_registered_input_keys(), 325 self._states[label].get_registered_output_keys(), 326 self._remappings[label])) 327 except: 328 self._user_code_exception = True 329 with self._done_cond: 330 self._done_cond.notify_all() 331 raise smach.InvalidStateError(("Could not execute child state '%s': " % label)+traceback.format_exc()) 332 333 # Make sure the child returned an outcome 334 if self._child_outcomes[label] is None: 335 raise smach.InvalidStateError("Concurrent state '%s' returned no outcome on termination." % label) 336 else: 337 smach.loginfo("Concurrent state '%s' returned outcome '%s' on termination." % (label, self._child_outcomes[label])) 338 339 # Check if all of the states have completed 340 with self._done_cond: 341 # initialize preemption flag 342 preempt_others = False 343 # Call transition cb's 344 self.call_transition_cbs() 345 # Call child termination cb if it's defined 346 if self._child_termination_cb: 347 try: 348 preempt_others = self._child_termination_cb(self._child_outcomes) 349 except: 350 raise smach.InvalidUserCodeError("Could not execute child termination callback: "+traceback.format_exc()) 351 352 # Notify the container to terminate (and preempt other states if neceesary) 353 if preempt_others or all([o is not None for o in self._child_outcomes.values()]): 354 self._done_cond.notify_all()

355 356 ### Container interface

357 - def get_children(self):

358 return self._states

359

360 - def __getitem__(self,key):

361 return self._states[key]

362

363 - def get_initial_states(self):

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

365

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

367 if initial_states > 0: 368 if initial_states < len(self._states): 369 smach.logwarn("Attempting to set initial states in Concurrence" 370 " container, but Concurrence children are always" 371 " all executed initially, ignoring call.") 372 373 # Set local userdata 374 self.userdata.update(userdata)

375

376 - def get_active_states(self):

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

378

379 - def get_internal_edges(self):

380 int_edges = [] 381 for (container_outcome, outcomes) in ((k,self._outcome_map[k]) for k in self._outcome_map): 382 for state_key in outcomes: 383 int_edges.append((outcomes[state_key], state_key, container_outcome)) 384 return int_edges

385

386 - def check_consistency(self):

387 for (co,cso) in ((k,self._outcome_map[k]) for k in self._outcome_map): 388 for state_label,outcome in ((k,cso[k]) for k in cso): 389 if outcome not in self._states[state_label].get_registered_outcomes(): 390 raise smach.InvalidTransitionError( 391 'Outcome map in SMACH Concurrence references a state outcome that does not exist. Requested state outcome: \'%s\', but state \'%s\' only has outcomes %s' % 392 (outcome, state_label, str(self._states[state_label].get_registered_outcomes())))

Source Code for Module smach.concurrence