def _operatable_execute(self, *args, **kwargs):
outcome = self.__execute(*args, **kwargs)
if self._is_controlled:
# reset previously requested outcome if applicable
if self._last_requested_outcome is not None and outcome == OperatableState._loopback_name:
self._pub.publish(self._request_topic, OutcomeRequest(outcome=255, target=self._parent._get_path() + "/" + self.name))
self._last_requested_outcome = None
# request outcome because autonomy level is too low
if not self._force_transition and (self.autonomy.has_key(outcome) and self.autonomy[outcome] >= OperatableStateMachine.autonomy_level or
outcome != OperatableState._loopback_name and
self._get_path() in rospy.get_param('/flexbe/breakpoints', [])):
if outcome != self._last_requested_outcome:
self._pub.publish(self._request_topic, OutcomeRequest(outcome=self._outcome_list.index(outcome), target=self._parent._get_path() + "/" + self.name))
rospy.loginfo("<-- Want result: %s > %s", self.name, outcome)
StateLogger.log_state_execution(self._get_path(), self.__class__.__name__, outcome, not self._force_transition, False)
self._last_requested_outcome = outcome
outcome = OperatableState._loopback_name
# autonomy level is high enough, report the executed transition
elif outcome != OperatableState._loopback_name:
self._sent_outcome_requests = []
rospy.loginfo("State result: %s > %s", self.name, outcome)
self._pub.publish(self._outcome_topic, UInt8(self._outcome_list.index(outcome)))
self._pub.publish(self._debug_topic, String("%s > %s" % (self._get_path(), outcome)))
StateLogger.log_state_execution(self._get_path(), self.__class__.__name__, outcome, not self._force_transition, True)
self._force_transition = False
return outcome
def _operatable_execute(self, *args, **kwargs):
outcome = self.__execute(*args, **kwargs)
if self._is_controlled:
# reset previously requested outcome if applicable
if self._last_requested_outcome is not None and outcome is None:
self._pub.publish(
self._request_topic,
OutcomeRequest(outcome=255, target=self.path))
self._last_requested_outcome = None
# request outcome because autonomy level is too low
if not self._force_transition and (
not self.parent.is_transition_allowed(self.name, outcome)
or outcome is not None and self.is_breakpoint):
if outcome != self._last_requested_outcome:
self._pub.publish(
self._request_topic,
OutcomeRequest(outcome=self.outcomes.index(outcome),
target=self.path))
Logger.localinfo("<-- Want result: %s > %s" %
(self.name, outcome))
StateLogger.log(
'flexbe.operator',
self,
type='request',
request=outcome,
autonomy=self.parent.autonomy_level,
required=self.parent.get_required_autonomy(outcome))
self._last_requested_outcome = outcome
outcome = None
# autonomy level is high enough, report the executed transition
elif outcome is not None and outcome in self.outcomes:
Logger.localinfo("State result: %s > %s" %
(self.name, outcome))
self._pub.publish(self._outcome_topic,
UInt8(self.outcomes.index(outcome)))
self._pub.publish(self._debug_topic,
String("%s > %s" % (self.path, outcome)))
if self._force_transition:
StateLogger.log('flexbe.operator',
self,
type='forced',
forced=outcome,
requested=self._last_requested_outcome)
self._last_requested_outcome = None
self._force_transition = False
return outcome
def test_cross_combinations(self):
state = self._create()
# manual transition works on low autonomy
OperatableStateMachine.autonomy_level = 0
state.result = 'error'
outcome = self._execute(state)
self.assertIsNone(outcome)
state._sub._callback(OutcomeRequest(target='subject', outcome=0),
'flexbe/command/transition')
outcome = self._execute(state)
self.assertEqual(outcome, 'done')
OperatableStateMachine.autonomy_level = 3
state.result = None
# manual transition blocked by lock
state._sub._callback(String('/subject'), 'flexbe/command/lock')
outcome = self._execute(state)
self.assertIsNone(outcome)
state._sub._callback(OutcomeRequest(target='subject', outcome=1),
'flexbe/command/transition')
outcome = self._execute(state)
self.assertIsNone(outcome)
state._sub._callback(String('/subject'), 'flexbe/command/unlock')
outcome = self._execute(state)
self.assertEqual(outcome, 'error')
# preempt works on low autonomy
OperatableStateMachine.autonomy_level = 0
state.result = 'error'
outcome = self._execute(state)
self.assertIsNone(outcome)
state._sub._callback(Empty(), 'flexbe/command/preempt')
outcome = self._execute(state)
self.assertEqual(outcome, PreemptableState._preempted_name)
PreemptableState.preempt = False
OperatableStateMachine.autonomy_level = 3
state.result = None
# preempt also works when locked
state._sub._callback(String('/subject'), 'flexbe/command/lock')
outcome = self._execute(state)
self.assertIsNone(outcome)
state._sub._callback(Empty(), 'flexbe/command/preempt')
outcome = self._execute(state)
self.assertEqual(outcome, PreemptableState._preempted_name)
PreemptableState.preempt = False
state._sub._callback(String('/subject'), 'flexbe/command/unlock')
outcome = self._execute(state)
self.assertIsNone(outcome)
def test_operatable_state(self):
state = self._create()
out_topic = 'flexbe/mirror/outcome'
req_topic = 'flexbe/outcome_request'
sub = ProxySubscriberCached({
out_topic: UInt8,
req_topic: OutcomeRequest
})
rospy.sleep(0.2) # wait for pub/sub
# return outcome in full autonomy, no request
state.result = 'error'
self._execute(state)
self.assertNoMessage(sub, req_topic)
self.assertMessage(sub, out_topic, UInt8(1))
# request outcome on same autnomy and clear request on loopback
OperatableStateMachine.autonomy_level = 2
self._execute(state)
self.assertNoMessage(sub, out_topic)
self.assertMessage(sub, req_topic,
OutcomeRequest(outcome=1, target='/subject'))
state.result = None
self._execute(state)
self.assertMessage(sub, req_topic,
OutcomeRequest(outcome=255, target='/subject'))
# still return other outcomes
state.result = 'done'
self._execute(state)
self.assertNoMessage(sub, req_topic)
self.assertMessage(sub, out_topic, UInt8(0))
# request outcome on lower autonomy, return outcome after level increase
OperatableStateMachine.autonomy_level = 0
self._execute(state)
self.assertNoMessage(sub, out_topic)
self.assertMessage(sub, req_topic,
OutcomeRequest(outcome=0, target='/subject'))
OperatableStateMachine.autonomy_level = 3
self._execute(state)
self.assertMessage(sub, out_topic, UInt8(0))
def test_manually_transitionable_state(self):
state = self._create()
fb_topic = 'flexbe/command_feedback'
sub = ProxySubscriberCached({fb_topic: CommandFeedback})
rospy.sleep(0.2) # wait for pub/sub
# return requested outcome
state._sub._callback(OutcomeRequest(target='subject', outcome=1),
'flexbe/command/transition')
outcome = self._execute(state)
self.assertEqual(outcome, 'error')
self.assertMessage(
sub, fb_topic,
CommandFeedback(command='transition', args=['subject', 'subject']))
# reject outcome request for different target
state._sub._callback(OutcomeRequest(target='invalid', outcome=1),
'flexbe/command/transition')
outcome = self._execute(state)
self.assertIsNone(outcome)
self.assertMessage(
sub, fb_topic,
CommandFeedback(command='transition', args=['invalid', 'subject']))
def _operatable_execute(self, *args, **kwargs):
outcome = self.__execute(*args, **kwargs)
if self._is_controlled:
log_requested_outcome = outcome
# request outcome because autonomy level is too low
if not self._force_transition and (
self.autonomy.has_key(outcome) and self.autonomy[outcome]
>= OperatableStateMachine.autonomy_level):
if self._sent_outcome_requests.count(outcome) == 0:
self._pub.publish(
self._request_topic,
OutcomeRequest(
outcome=self._outcome_list.index(outcome),
target=self._parent._get_path() + "/" + self.name))
rospy.loginfo("<-- Want result: %s > %s", self.name,
outcome)
StateLogger.log_state_execution(self._get_path(),
self.__class__.__name__,
outcome,
not self._force_transition,
False)
self._sent_outcome_requests.append(outcome)
outcome = OperatableState._loopback_name
# autonomy level is high enough, report the executed transition
elif outcome != OperatableState._loopback_name:
self._sent_outcome_requests = []
rospy.loginfo("State result: %s > %s", self.name, outcome)
self._pub.publish(self._outcome_topic,
UInt8(self._outcome_list.index(outcome)))
self._pub.publish(
self._debug_topic,
String("%s > %s" % (self._get_path(), outcome)))
StateLogger.log_state_execution(self._get_path(),
self.__class__.__name__,
outcome,
not self._force_transition,
True)
self._force_transition = False
return outcome