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/request_mirror_structure', Int32, self._mirror_structure_callback)
StateLogger.initialize(name)
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 destroy(self):
self._notify_stop()
self._disable_ros_control()
self._sub.unsubscribe_topic('flexbe/command/autonomy')
self._sub.unsubscribe_topic('flexbe/command/sync')
self._sub.unsubscribe_topic('flexbe/request_mirror_structure')
StateLogger.shutdown()
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/request_mirror_structure', Int32, self._mirror_structure_callback)
StateLogger.initialize(name)
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 destroy(self):
self._notify_stop()
self._disable_ros_control()
self._sub.unsubscribe_topic('/flexbe/command/autonomy')
self._sub.unsubscribe_topic('/flexbe/command/sync')
self._sub.unsubscribe_topic('/flexbe/request_mirror_structure')
StateLogger.shutdown()
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)))
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 == 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 _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
