concurrency_container.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 import traceback
3 import smach
4 
5 from smach.state_machine import StateMachine
6 
7 from flexbe_core.core.event_state import EventState
8 from flexbe_core.core.operatable_state_machine import OperatableStateMachine
9 from flexbe_core.core.preemptable_state import PreemptableState
10 from flexbe_core.core.priority_container import PriorityContainer
11 
12 
14  """
15  A state machine that can be operated.
16  It synchronizes its current state with the mirror and supports some control mechanisms.
17  """
18 
19  def __init__(self, conditions=dict(), *args, **kwargs):
20  super(ConcurrencyContainer, self).__init__(*args, **kwargs)
21  self._conditions = conditions
22  self._returned_outcomes = dict()
23 
24  self.__execute = self.execute
26 
27 
28  def _concurrency_execute(self, *args, **kwargs):
29  # use state machine execute, not state execute
30  return OperatableStateMachine.execute(self, *args, **kwargs)
31 
32 
33  def _build_msg(self, *args, **kwargs):
34  # use state machine _build_msg, not state execute
35  return OperatableStateMachine._build_msg(self, *args, **kwargs)
36 
37 
38  def _update_once(self):
39  # Check if a preempt was requested before or while the last state was running
40  if self.preempt_requested() or PreemptableState.preempt:
41  return self._preempted_name
42 
43  #self._state_transitioning_lock.release()
44  for state in self._ordered_states:
45  if state.name in self._returned_outcomes.keys() and self._returned_outcomes[state.name] != self._loopback_name:
46  continue
47  if PriorityContainer.active_container is not None \
48  and not PriorityContainer.active_container.startswith(state._get_path()) \
49  and not state._get_path().startswith(PriorityContainer.active_container):
50  if isinstance(state, EventState):
51  state._notify_skipped()
52  elif state._get_deep_state() is not None:
53  state._get_deep_state()._notify_skipped()
54  continue
55  self._returned_outcomes[state.name] = self._execute_state(state)
56  #self._state_transitioning_lock.acquire()
57 
58  # Determine outcome
59  outcome = self._loopback_name
60  for item in self._conditions:
61  (oc, cond) = item
62  if all(self._returned_outcomes.has_key(sn) and self._returned_outcomes[sn] == o for sn,o in cond):
63  outcome = oc
64  break
65 
66  # preempt (?)
67  if outcome == self._loopback_name:
68  return None
69 
70  if outcome in self.get_registered_outcomes():
71  # Call termination callbacks
72  self.call_termination_cbs([s.name for s in self._ordered_states],outcome)
73  self.on_exit(self.userdata, states = filter(lambda s: s.name not in self._returned_outcomes.keys() or self._returned_outcomes[s.name] == self._loopback_name, self._ordered_states))
74  self._returned_outcomes = dict()
75  # right now, going out of a cc may break sync
76  # thus, as a quick fix, explicitly sync again
77  self._parent._inner_sync_request = True
78  self._current_state = None
79 
80  return outcome
81  else:
82  raise smach.InvalidTransitionError("Outcome '%s' of state '%s' with transition target '%s' is neither a registered state nor a registered container outcome." %
83  (outcome, self.name, outcome))
84 
85  def _execute_state(self, state, force_exit=False):
86  result = None
87  try:
88  ud = smach.Remapper(
89  self.userdata,
90  state.get_registered_input_keys(),
91  state.get_registered_output_keys(),
92  self._remappings[state.name])
93  if force_exit:
94  state._entering = True
95  state.on_exit(ud)
96  else:
97  result = state.execute(ud)
98  #print 'execute %s --> %s' % (state.name, self._returned_outcomes[state.name])
99  except smach.InvalidUserCodeError as ex:
100  smach.logerr("State '%s' failed to execute." % state.name)
101  raise ex
102  except:
103  raise smach.InvalidUserCodeError("Could not execute state '%s' of type '%s': " %
104  (state.name, state)
105  + traceback.format_exc())
106  return result
107 
108 
109  def _notify_start(self):
110  for state in self._ordered_states:
111  if isinstance(state, EventState):
112  state.on_start()
113  if isinstance(state, OperatableStateMachine):
114  state._notify_start()
115 
117  state = self._ordered_states[0]
118  if isinstance(state, EventState):
119  state._enable_ros_control()
120  if isinstance(state, OperatableStateMachine):
121  state._enable_ros_control()
122 
123  def _notify_stop(self):
124  for state in self._ordered_states:
125  if isinstance(state, EventState):
126  state.on_stop()
127  if isinstance(state, OperatableStateMachine):
128  state._notify_stop()
129  if state._is_controlled:
130  state._disable_ros_control()
131 
133  state = self._ordered_states[0]
134  if isinstance(state, EventState):
135  state._disable_ros_control()
136  if isinstance(state, OperatableStateMachine):
137  state._disable_ros_control()
138 
139  def on_exit(self, userdata, states = None):
140  for state in self._ordered_states if states is None else states:
141  self._execute_state(state, force_exit=True)


flexbe_core
Author(s): Philipp Schillinger
autogenerated on Wed Jun 5 2019 21:51:59