Python ProxyPublisher.createPublisher方法代码示例

# 需要导入模块: from flexbe_core.proxy import ProxyPublisher [as 别名]
# 或者: from flexbe_core.proxy.ProxyPublisher import createPublisher [as 别名]
class PreemptableState(LoopbackState):
    """
    A state that can be preempted.
    If preempted, the state will not be executed anymore and return the outcome preempted.
    """

    _preempted_name = "preempted"
    preempt = False
    switching = False

    def __init__(self, *args, **kwargs):
        # add preempted outcome
        if len(args) > 0 and type(args[0]) is list:
            # need this ugly check for list type, because first argument in CBState is the callback
            args[0].append(self._preempted_name)
        else:
            outcomes = kwargs.get("outcomes", [])
            outcomes.append(self._preempted_name)
            kwargs["outcomes"] = outcomes

        super(PreemptableState, self).__init__(*args, **kwargs)
        self.__execute = self.execute
        self.execute = self._preemptable_execute

        self._feedback_topic = "/flexbe/command_feedback"
        self._preempt_topic = "/flexbe/command/preempt"

        self._pub = ProxyPublisher()
        self._sub = ProxySubscriberCached()

    def _preemptable_execute(self, *args, **kwargs):
        preempting = False
        if self._is_controlled and self._sub.has_msg(self._preempt_topic):
            self._sub.remove_last_msg(self._preempt_topic)
            self._pub.publish(self._feedback_topic, CommandFeedback(command="preempt"))
            preempting = True
            rospy.loginfo("--> Behavior will be preempted")

        if PreemptableState.preempt:
            PreemptableState.preempt = False
            preempting = True
            rospy.loginfo("Behavior will be preempted")

        if preempting:
            self.service_preempt()
            self._force_transition = True
            return self._preempted_name

        return self.__execute(*args, **kwargs)

    def _enable_ros_control(self):
        super(PreemptableState, self)._enable_ros_control()
        self._pub.createPublisher(self._feedback_topic, CommandFeedback)
        self._sub.subscribe(self._preempt_topic, Empty)
        PreemptableState.preempt = False

    def _disable_ros_control(self):
        super(PreemptableState, self)._disable_ros_control()
        self._sub.unsubscribe_topic(self._preempt_topic)

# 需要导入模块: from flexbe_core.proxy import ProxyPublisher [as 别名]
# 或者: from flexbe_core.proxy.ProxyPublisher import createPublisher [as 别名]
class ManuallyTransitionableState(MonitoringState):
    """
    A state for that a desired outcome can be declared.
    If any outcome is declared, this outcome is forced.
    """
    
    def __init__(self, *args, **kwargs):
        super(ManuallyTransitionableState, self).__init__(*args, **kwargs)
        
        self._force_transition = False
        
        self.__execute = self.execute
        self.execute = self._manually_transitionable_execute

        self._feedback_topic = '/flexbe/command_feedback'
        self._transition_topic = '/flexbe/command/transition'

        self._pub = ProxyPublisher()
        self._sub = ProxySubscriberCached()


    def _manually_transitionable_execute(self, *args, **kwargs):
        if self._is_controlled and self._sub.has_buffered(self._transition_topic):
            command_msg = self._sub.get_from_buffer(self._transition_topic)
            self._pub.publish(self._feedback_topic, CommandFeedback(command="transition", args=[command_msg.target, self.name]))
            if command_msg.target != self.name:
                rospy.logwarn("--> Requested outcome for state " + command_msg.target + " but active state is " + self.name)
            else:
                self._force_transition = True
                outcome = self._outcome_list[command_msg.outcome]
                rospy.loginfo("--> Manually triggered outcome " + outcome + " of state " + self.name)
                return outcome
        
        # return the normal outcome
        self._force_transition = False
        return self.__execute(*args, **kwargs)


    def _enable_ros_control(self):
        super(ManuallyTransitionableState, self)._enable_ros_control()
        self._pub.createPublisher(self._feedback_topic, CommandFeedback)
        self._sub.subscribe(self._transition_topic, OutcomeRequest)
        self._sub.enable_buffer(self._transition_topic)

    def _disable_ros_control(self):
        super(ManuallyTransitionableState, self)._disable_ros_control()
        self._sub.unsubscribe_topic(self._transition_topic)

# 需要导入模块: from flexbe_core.proxy import ProxyPublisher [as 别名]
# 或者: from flexbe_core.proxy.ProxyPublisher import createPublisher [as 别名]
class LockableState(ManuallyTransitionableState):
    """
    A state that can be locked.
    When locked, no transition can be done regardless of the resulting outcome.
    However, if any outcome would be triggered, the outcome will be stored
    and the state won't be executed anymore until it is unlocked and the stored outcome is set.
    """
    
    def __init__(self, *args, **kwargs):
        super(LockableState, self).__init__(*args, **kwargs)

        self._locked = False
        self._stored_outcome = None
        
        self.__execute = self.execute
        self.execute = self._lockable_execute

        self._feedback_topic = 'flexbe/command_feedback'
        self._lock_topic = 'flexbe/command/lock'
        self._unlock_topic = 'flexbe/command/unlock'

        self._pub = ProxyPublisher()
        self._sub = ProxySubscriberCached()


    def _lockable_execute(self, *args, **kwargs):
        if self._is_controlled and self._sub.has_msg(self._lock_topic):
            msg = self._sub.get_last_msg(self._lock_topic)
            self._sub.remove_last_msg(self._lock_topic)
            self._execute_lock(msg.data)

        if self._is_controlled and self._sub.has_msg(self._unlock_topic):
            msg = self._sub.get_last_msg(self._unlock_topic)
            self._sub.remove_last_msg(self._unlock_topic)
            self._execute_unlock(msg.data)

        if self._locked:
            if self._stored_outcome is None or self._stored_outcome == 'None':
                self._stored_outcome = self.__execute(*args, **kwargs)
            return None
            
        if not self._locked and not self._stored_outcome is None and not self._stored_outcome == 'None':
            if self._parent.transition_allowed(self.name, self._stored_outcome):
                outcome = self._stored_outcome
                self._stored_outcome = None
                return outcome
            else:
                return None

        outcome = self.__execute(*args, **kwargs)

        if outcome is None or outcome == 'None':
            return None

        if not self._parent is None and not self._parent.transition_allowed(self.name, outcome):
            self._stored_outcome = outcome
            return None

        return outcome


    def _execute_lock(self, target):
        found_target = False
        if target == self._get_path():
            found_target = True
            self._locked = True
        else:
            found_target = self._parent.lock(target)

        self._pub.publish(self._feedback_topic, CommandFeedback(command="lock", args=[target, target if found_target else ""]))
        if not found_target:
            rospy.logwarn("--> Wanted to lock %s, but could not find it in current path %s.", target, self._get_path())
        else:
            rospy.loginfo("--> Locking in state %s", target)


    def _execute_unlock(self, target):
        found_target = False
        if target == self._get_path():
            found_target = True
            self._locked = False
        else:
            found_target = self._parent.unlock(target)

        self._pub.publish(self._feedback_topic, CommandFeedback(command="unlock", args=[target, target if found_target else ""]))
        if not found_target:
            rospy.logwarn("--> Wanted to unlock %s, but could not find it in current path %s.", target, self._get_path())
        else:
            rospy.loginfo("--> Unlocking in state %s", target)


    def _enable_ros_control(self):
        super(LockableState, self)._enable_ros_control()
        self._pub.createPublisher(self._feedback_topic, CommandFeedback)
        self._sub.subscribe(self._lock_topic, String)
        self._sub.subscribe(self._unlock_topic, String)

    def _disable_ros_control(self):
        super(LockableState, self)._disable_ros_control()
        self._sub.unsubscribe_topic(self._lock_topic)
#.........这里部分代码省略.........

# 需要导入模块: from flexbe_core.proxy import ProxyPublisher [as 别名]
# 或者: from flexbe_core.proxy.ProxyPublisher import createPublisher [as 别名]
class EventState(OperatableState):
    """
    A state that allows implementing certain events.
    """

    def __init__(self, *args, **kwargs):
        super(EventState, self).__init__(*args, **kwargs)

        self._entering = True
        self._skipped = False
        self.__execute = self.execute
        self.execute = self._event_execute

        self._paused = False
        self._last_active_container = None

        self._feedback_topic = "flexbe/command_feedback"
        self._repeat_topic = "flexbe/command/repeat"
        self._pause_topic = "flexbe/command/pause"

        self._pub = ProxyPublisher()
        self._sub = ProxySubscriberCached()

    def _event_execute(self, *args, **kwargs):
        if self._is_controlled and self._sub.has_msg(self._pause_topic):
            msg = self._sub.get_last_msg(self._pause_topic)
            if msg.data:
                self._sub.remove_last_msg(self._pause_topic)
                rospy.loginfo("--> Pausing in state %s", self.name)
                self._pub.publish(self._feedback_topic, CommandFeedback(command="pause"))
                self._last_active_container = PriorityContainer.active_container
                PriorityContainer.active_container = ""
                self._paused = True

        if self._paused and not PreemptableState.preempt:
            self._notify_skipped()
            return self._loopback_name

        if self._entering:
            self._entering = False
            self.on_enter(*args, **kwargs)
        if self._skipped:
            self._skipped = False
            self.on_resume(*args, **kwargs)

        execute_outcome = self.__execute(*args, **kwargs)

        repeat = False
        if self._is_controlled and self._sub.has_msg(self._repeat_topic):
            rospy.loginfo("--> Repeating state %s", self.name)
            self._sub.remove_last_msg(self._repeat_topic)
            self._pub.publish(self._feedback_topic, CommandFeedback(command="repeat"))
            repeat = True

        if execute_outcome != self._loopback_name and not PreemptableState.switching or repeat:
            self._entering = True
            self.on_exit(*args, **kwargs)

        return execute_outcome

    def _notify_skipped(self):
        if not self._skipped:
            self.on_pause()
            self._skipped = True
        if self._is_controlled and self._sub.has_msg(self._pause_topic):
            msg = self._sub.get_last_msg(self._pause_topic)
            if not msg.data:
                self._sub.remove_last_msg(self._pause_topic)
                rospy.loginfo("--> Resuming in state %s", self.name)
                self._pub.publish(self._feedback_topic, CommandFeedback(command="resume"))
                PriorityContainer.active_container = self._last_active_container
                self._last_active_container = None
                self._paused = False
        super(EventState, self)._notify_skipped()

    def _enable_ros_control(self):
        super(EventState, self)._enable_ros_control()
        self._pub.createPublisher(self._feedback_topic, CommandFeedback)
        self._sub.subscribe(self._repeat_topic, Empty)
        self._sub.subscribe(self._pause_topic, Bool)

    def _disable_ros_control(self):
        super(EventState, self)._disable_ros_control()
        self._sub.unsubscribe_topic(self._repeat_topic)
        self._sub.unsubscribe_topic(self._pause_topic)
        self._last_active_container = None
        if self._paused:
            PriorityContainer.active_container = None

    # Events
    # (just implement the ones you need)

    def on_start(self):
        """
        Will be executed once when the behavior starts.
        """
        pass

    def on_stop(self):
        """
#.........这里部分代码省略.........

# 需要导入模块: from flexbe_core.proxy import ProxyPublisher [as 别名]
# 或者: from flexbe_core.proxy.ProxyPublisher import createPublisher [as 别名]
class OperatableStateMachine(PreemptableStateMachine):
    """
    A state machine that can be operated.
    It synchronizes its current state with the mirror and supports some control mechanisms.
    """
    
    autonomy_level = 3
    silent_mode = False
    
    def __init__(self, *args, **kwargs):
        super(OperatableStateMachine, self).__init__(*args, **kwargs)
        self._message = None

        self.id = None
        self.autonomy = None

        self._autonomy = {}
        self._ordered_states = []
        
        self._pub = ProxyPublisher()

        self._sub = ProxySubscriberCached()


    @staticmethod
    def add(label, state, transitions = None, autonomy = None, remapping = None):
        """
        Add a state to the opened state machine.
        
        @type label: string
        @param label: The label of the state being added.
        
        @param state: An instance of a class implementing the L{State} interface.
        
        @param transitions: A dictionary mapping state outcomes to other state
        labels or container outcomes.
        
        @param autonomy: A dictionary mapping state outcomes to their required
        autonomy level

        @param remapping: A dictionary mapping local userdata keys to userdata
        keys in the container.
        """
        self = StateMachine._currently_opened_container()
        
        # add loopback transition to loopback states
        if isinstance(state, LoopbackState):
            transitions[LoopbackState._loopback_name] = label
            autonomy[LoopbackState._loopback_name] = -1
            
        self._ordered_states.append(state)
        state.name = label
        state.transitions = transitions
        state.autonomy = autonomy
        state._parent = self
            
        StateMachine.add(label, state, transitions, remapping)
        self._autonomy[label] = autonomy

    def replace(self, new_state):
        old_state = self._states[new_state.name]
        new_state.transitions = old_state.transitions
        new_state.autonomy = old_state.autonomy
        new_state._parent = old_state._parent

        self._ordered_states[self._ordered_states.index(old_state)] = new_state
        self._states[new_state.name] = new_state
            
            
    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)

#.........这里部分代码省略.........

# 需要导入模块: from flexbe_core.proxy import ProxyPublisher [as 别名]
# 或者: from flexbe_core.proxy.ProxyPublisher import createPublisher [as 别名]
class OperatableState(PreemptableState):
    """
    A state that supports autonomy levels and silent mode.
    Also, it allows being tracked by a GUI or anything else
    as it reports each transition and its initial structure.
    An additional method is provided to report custom status messages to the widget.
    """
    
    def __init__(self, *args, **kwargs):
        super(OperatableState, self).__init__(*args, **kwargs)
        self.transitions = None
        self.autonomy = None
        
        self._mute = False  # is set to true when used in silent state machine (don't report transitions)
        self._sent_outcome_requests = []  # contains those outcomes that already requested a transition
        
        self._outcome_topic = '/flexbe/mirror/outcome'
        self._request_topic = '/flexbe/outcome_request'
        self._pub = ProxyPublisher()
        
        self.__execute = self.execute
        self.execute = self._operatable_execute
        
        
    def _build_msg(self, prefix, msg):
        """
        Adds this state to the initial structure message.
        
        @type prefix: string
        @param prefix: A path consisting of the container hierarchy containing this state.
        
        @type msg: ContainerStructure
        @param msg: The message that will finally contain the structure message.
        """
        
        # set path
        name = prefix + self.name
        
        # no children
        children = None
        
        # set outcomes
        outcomes = self._outcome_list
        
        # set transitions and autonomy levels
        transitions = []
        autonomy = []
        for i in range(len(outcomes)):
            outcome = outcomes[i]
            if outcome == 'preempted':          # set preempt transition
                transitions.append('preempted')
                autonomy.append(-1)
            else:
                transitions.append(str(self.transitions[outcome]))
                autonomy.append(self.autonomy[outcome])
        
        # add to message
        msg.containers.append(Container(name, children, outcomes, transitions, autonomy))
        

    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 _enable_ros_control(self):
        super(OperatableState, self)._enable_ros_control()
        self._pub.createPublisher(self._outcome_topic, UInt8)
        self._pub.createPublisher(self._request_topic, OutcomeRequest)

# 需要导入模块: from flexbe_core.proxy import ProxyPublisher [as 别名]
# 或者: from flexbe_core.proxy.ProxyPublisher import createPublisher [as 别名]
class VigirBeOnboard(object):
    '''
    Implements an idle state where the robot waits for a behavior to be started.
    '''


    def __init__(self):
        '''
        Constructor
        '''
        self.be = None
        Logger.initialize()

        #ProxyPublisher._simulate_delay = True
        #ProxySubscriberCached._simulate_delay = True

        # prepare temp folder
        rp = rospkg.RosPack()
        self._tmp_folder = os.path.join(rp.get_path('flexbe_onboard'), 'tmp/')
        if not os.path.exists(self._tmp_folder):
            os.makedirs(self._tmp_folder)
        sys.path.append(self._tmp_folder)

        # prepare manifest folder access
        manifest_folder = os.path.join(rp.get_path('flexbe_behaviors'), 'behaviors/')
        rospy.loginfo("Parsing available behaviors...")
        file_entries = [os.path.join(manifest_folder, filename) for filename in os.listdir(manifest_folder) if not filename.startswith('#')]
        manifests = sorted([xmlpath for xmlpath in file_entries if not os.path.isdir(xmlpath)])
        self._behavior_lib = dict()
        for i in range(len(manifests)):
            m = ET.parse(manifests[i]).getroot()
            e = m.find("executable")
            self._behavior_lib[i] = {"name": m.get("name"), "package": e.get("package_path").split(".")[0], "file": e.get("package_path").split(".")[1], "class": e.get("class")}
#            rospy.loginfo("+++ " + self._behavior_lib[i]["name"])

        self._pub = ProxyPublisher()
        self._sub = ProxySubscriberCached()

        self.status_topic = '/flexbe/status'
        self.feedback_topic = '/flexbe/command_feedback'

        self._pub.createPublisher(self.status_topic, BEStatus, _latch = True)
        self._pub.createPublisher(self.feedback_topic, CommandFeedback)

        # listen for new behavior to start
        self._running = False
        self._switching = False
        self._sub.subscribe('/flexbe/start_behavior', BehaviorSelection, self._behavior_callback)

        # heartbeat
        self._pub.createPublisher('/flexbe/heartbeat', Empty)
        self._execute_heartbeat()

        rospy.sleep(0.5) # wait for publishers etc to really be set up
        self._pub.publish(self.status_topic, BEStatus(code=BEStatus.READY))
        rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m')

        
    def _behavior_callback(self, msg):
        thread = threading.Thread(target=self._behavior_execution, args=[msg])
        thread.daemon = True
        thread.start()


    def _behavior_execution(self, msg):
        if self._running:
            Logger.loginfo('--> Initiating behavior switch...')
            self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['received']))
        else:
            Logger.loginfo('--> Starting new behavior...')

        be = self._prepare_behavior(msg)
        if be is None:
            Logger.logerr('Dropped behavior start request because preparation failed.')
            if self._running:
                self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['failed']))
            else:
                rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m')
            return

        if self._running:
            if self._switching:
                Logger.logwarn('Already switching, dropped new start request.')
                return
            self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['start']))
            if not self._is_switchable(be):
                Logger.logerr('Dropped behavior start request because switching is not possible.')
                self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['not_switchable']))
                return
            self._switching = True
            active_state = self.be.get_current_state()
            rospy.loginfo("Current state %s is kept active.", active_state.name)
            try:
                be.prepare_for_switch(active_state)
                self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['prepared']))
            except Exception as e:
                Logger.logerr('Failed to prepare behavior switch:\n%s' % str(e))
                self._switching = False
                self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['failed']))
                return
#.........这里部分代码省略.........