def _execute_current_state(self):
# catch any exception and keep state active to let operator intervene
try:
outcome = super(OperatableStateMachine,
self)._execute_current_state()
self._last_exception = None
except Exception as e:
outcome = None
self._last_exception = e
Logger.logerr('Failed to execute state %s:\n%s' %
(self.current_state_label, str(e)))
# provide explicit sync as back-up functionality
# should be used only if there is no other choice
# since it requires additional 8 byte + header update bandwith and time to restart mirror
if self._inner_sync_request and self.get_deep_state() is not None:
self._inner_sync_request = False
if self.id is None:
self.parent._inner_sync_request = True
else:
msg = BehaviorSync()
msg.behavior_id = self.id
msg.current_state_checksum = zlib.adler32(
self.get_deep_state().path.encode()) & 0x7fffffff
self._pub.publish('flexbe/mirror/sync', msg)
return outcome
def _async_execute(self, parent_ud=smach.UserData()):
"""Run the state machine on entry to this state.
This will set the "closed" flag and spin up the execute thread. Once
this flag has been set, it will prevent more states from being added to
the state machine.
"""
# This will prevent preempts from getting propagated to non-existent children
with self._state_transitioning_lock:
# Check state consistency
try:
self.check_consistency()
except (smach.InvalidStateError, smach.InvalidTransitionError):
smach.logerr("Container consistency check failed.")
return None
# Set running flag
self._is_running = True
# Initialize container outcome
container_outcome = None
# Step through state machine
if self._is_running and not smach.is_shutdown():
# Update the state machine
container_outcome = self._update_once()
if self._do_rate_sleep and self._current_state is not None and not isinstance(
self._current_state, OperatableStateMachine):
try:
# sleep with the rate of the state and update container rate accordingly
self._rate = self._current_state._rate
self._rate.sleep()
except ROSInterruptException:
rospy.logwarn('Interrupted rate sleep.')
if container_outcome is not None and container_outcome != self._loopback_name:
# Copy output keys
self._copy_output_keys(self.userdata, parent_ud)
else:
container_outcome = self._loopback_name
# provide explicit sync as back-up functionality
# should be used only if there is no other choice
# since it requires additional 8 byte + header update bandwith and time to restart mirror
if self._inner_sync_request and self._get_deep_state() is not None:
self._inner_sync_request = False
if self.id is None:
self._parent._inner_sync_request = True
else:
msg = BehaviorSync()
msg.behavior_id = self.id
msg.current_state_checksum = zlib.adler32(
self._get_deep_state()._get_path())
self._pub.publish('flexbe/mirror/sync', msg)
# We're no longer running
self._is_running = False
return container_outcome
def _sync_callback(self, msg):
rospy.loginfo("--> Synchronization requested...")
msg = BehaviorSync()
msg.behavior_id = self.id
msg.current_state_checksum = zlib.adler32(self._get_deep_state()._get_path())
self._pub.publish('/flexbe/mirror/sync', msg)
self._pub.publish('/flexbe/command_feedback', CommandFeedback(command="sync", args=[]))
rospy.loginfo("<-- Sent synchronization message for mirror.")
def _sync_callback(self, msg):
rospy.loginfo("--> Synchronization requested...")
msg = BehaviorSync()
msg.behavior_id = self.id
msg.current_state_checksum = zlib.adler32(self._get_deep_state()._get_path())
self._pub.publish('/flexbe/mirror/sync', msg)
self._pub.publish('/flexbe/command_feedback', CommandFeedback(command="sync", args=[]))
rospy.loginfo("<-- Sent synchronization message for mirror.")
def _sync_callback(self, msg):
Logger.localinfo("--> Synchronization requested...")
msg = BehaviorSync()
msg.behavior_id = self.id
# make sure we are already executing
self.wait(condition=lambda: self.get_deep_state() is not None)
msg.current_state_checksum = zlib.adler32(
self.get_deep_state().path.encode()) & 0x7fffffff
self._pub.publish('flexbe/mirror/sync', msg)
self._pub.publish('flexbe/command_feedback',
CommandFeedback(command="sync", args=[]))
Logger.localinfo("<-- Sent synchronization message for mirror.")