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 confirm(self, name, id):
"""
Confirms the state machine and triggers the creation of the structural message.
It is mandatory to call this function at the top-level state machine
between building it and starting its execution.
@type name: string
@param name: The name of this state machine to identify it.
"""
self.name = name
self.id = id
self._pub.createPublisher(
'flexbe/mirror/sync', BehaviorSync, _latch=True
) # Update mirror with currently active state (high bandwidth mode)
self._pub.createPublisher('flexbe/mirror/preempt', Empty,
_latch=True) # Preempts the mirror
self._pub.createPublisher(
'flexbe/mirror/structure',
ContainerStructure) # Sends the current structure to the mirror
self._pub.createPublisher('flexbe/log',
BehaviorLog) # Topic for logs to the GUI
self._pub.createPublisher(
'flexbe/command_feedback', CommandFeedback
) # Gives feedback about executed commands to the GUI
self._sub.subscribe('flexbe/command/autonomy', UInt8,
self._set_autonomy_level)
self._sub.subscribe('flexbe/command/sync', Empty, self._sync_callback)
self._sub.subscribe('flexbe/command/attach', UInt8,
self._attach_callback)
self._sub.subscribe('flexbe/request_mirror_structure', Int32,
self._mirror_structure_callback)
StateLogger.initialize(name)
StateLogger.log('flexbe.initialize',
None,
behavior=name,
autonomy=OperatableStateMachine.autonomy_level)
if OperatableStateMachine.autonomy_level != 255:
self._enable_ros_control()
rospy.sleep(0.5) # no clean way to wait for publisher to be ready...
self._notify_start()
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 (outcome in self.autonomy 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('flexbe.operator', self, type='request', request=outcome,
autonomy=OperatableStateMachine.autonomy_level,
required=self.autonomy[outcome])
self._last_requested_outcome = outcome
outcome = OperatableState._loopback_name
# autonomy level is high enough, report the executed transition
elif outcome != OperatableState._loopback_name:
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)))
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
