6 from flexbe_msgs.msg
import OutcomeRequest
7 from ..proxy
import ProxySubscriberCached
9 from .silent_state_machine
import SilentStateMachine
14 A state machine that runs in background and does not report any transition. 15 It can jump to any of its states, executing all states on a random way to the target 16 (so make sure its states have no side effects). 21 super(JumpableStateMachine, self).
__init__(*args, **kwargs)
23 self.
_sub = ProxySubscriberCached()
28 Jumps to the specified state. 30 @type target_state_label: string 31 @param target_state_label: The name of the state to jump to. 32 Should be unique. If not, the first occurrence will be chosen. 36 while isinstance(container._current_state, smach.StateMachine):
37 container = container._current_state
38 current_state = container._current_state
41 path = self.
_search_state(current_state, target_state_label, container, [],
'')
43 rospy.logerr(
'No path found from '+current_state.name+
' to '+target_state_label)
46 rospy.loginfo(
'Path: '+path)
48 JumpableStateMachine.refresh =
True 52 outcomes = path.split(
'/')
53 for i
in range(len(outcomes)-1):
54 current_state = self._get_deep_state()
55 current_state._requested_outcome = outcomes[i]
57 rospy.logerr(
'Unable to jump to '+target_state_label+
': Stuck in state '+current_state.name)
61 def _search_state(self, origin_state, target_state_label, container, seen_states, path):
63 Searches the given state to create an outcome path to it. 64 This is implemented by a hierarchical depth-first search. 66 @type origin_state: State 67 @param origin_state: The current origin for this search. 69 @type target_state_label: string 70 @param target_state_label: The name of the state to search. 72 @type container: StateMachine 73 @param container: The current container inside which the search is currently done. 75 @type seen_states: list of State 76 @param seen_states: A list of all states that already have been traversed. 79 @param path: The outcome path to the current origin_state 81 @return: The whole outcome path to the searched state. 84 seen_states.append(origin_state)
87 if origin_state.name == target_state_label:
92 if isinstance(origin_state, smach.StateMachine):
94 if origin_state._initial_state_label == target_state_label:
97 child_state = origin_state._states[origin_state._initial_state_label]
98 if seen_states.count(child_state) == 0:
99 target_path = self.
_search_state(child_state, target_state_label, origin_state, seen_states, path)
101 if target_path
is not None:
106 for outcome
in origin_state._outcomes:
107 next_state_label = origin_state.transitions[outcome]
108 if next_state_label ==
'preempted' or next_state_label ==
'preempted_mirror':
continue 109 next_container = container
114 while not next_container._states.has_key(next_state_label):
115 if next_container._parent
is None:
118 next_state_label = next_container.transitions[next_state_label]
119 next_container = next_container._parent
121 if reached_exit:
break 124 next_state = next_container._states[next_state_label]
125 if seen_states.count(next_state) == 0:
126 target_path = self.
_search_state(next_state, target_state_label, next_container, seen_states, path+outcome+
'/')
128 if target_path
is not None:
137 Sleeps until the state machine proceeded with the next state. 139 @type current_state: State 140 @param current_state: The current state when this method is called. 143 @param timeout: The time in seconds when this method should stop waiting when no transition is done. 146 @param period: The time in seconds how often this method should check the current state. 148 @return: True if waiting was successful, False if current_state is still the same (timeout). 150 end_time = time.time() + timeout
151 while time.time() < end_time:
152 new_state = self._get_deep_state()
153 if new_state != current_state:
161 Forces the state machine to do a transition with a specified outcome. 163 @type msg: OutcomeRequest 164 @param msg: A message containing all relevant information. 166 state = self._get_deep_state()
171 rospy.logwarn(
'Updating mirror while it is not running!')
173 state = self._get_deep_state()
174 rospy.logwarn(
'Had to wait for mirror starting to request transition of state %s!', msg.target)
177 if msg.target != state.name:
179 state = self._get_deep_state()
180 if msg.target != state.name:
181 rospy.logwarn(
'Wrong state name: requested %s but is %s!', msg.target, state.name)
183 rospy.logwarn(
'Had to wait for state %s to request transition!', msg.target)
186 if msg.outcome == 255:
187 state._requested_outcome =
'preempted' 189 state._requested_outcome = state._outcome_list[msg.outcome]
def _search_state(self, origin_state, target_state_label, container, seen_states, path)
def _transition_callback(self, msg)
def _wait_for_next_state(self, current_state, timeout, period=0.05)
def __init__(self, args, kwargs)
def jump_to(self, target_state_label)