Example #1
0
    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
Example #6
0
    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
Example #7
0
    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