Exemplo n.º 1
0
 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
Exemplo n.º 2
0
    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
Exemplo n.º 3
0
 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.")
Exemplo n.º 5
0
 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.")