Python ProxySubscriberCached.has_buffered方法代码示例

# 需要导入模块: from flexbe_core.proxy import ProxySubscriberCached [as 别名]
# 或者: from flexbe_core.proxy.ProxySubscriberCached import has_buffered [as 别名]
class GetJointValuesDynState(EventState):
	'''
	Retrieves current values of specified joints.

	># joint_names	string[]	List of desired joint names.

	#> joint_values float[] 	List of current joint values.

	<= retrieved 				Joint values are available.

	'''

	def __init__(self):
		'''
		Constructor
		'''
		super(GetJointValuesDynState, self).__init__(
			outcomes=['retrieved'],
			output_keys=['joint_values'],
			input_keys=['joint_names'])
		
		self._topic = '/joint_states'
		self._sub = ProxySubscriberCached({self._topic: JointState})

		self._joints = None
		self._joint_values = list()
			
		
	def execute(self, userdata):
		while self._sub.has_buffered(self._topic):
			msg = self._sub.get_from_buffer(self._topic)
			for i in range(len(msg.name)):
				if msg.name[i] in self._joints \
				and self._joint_values[self._joints.index(msg.name[i])] is None:
					self._joint_values[self._joints.index(msg.name[i])] = msg.position[i]

		if all(v is not None for v in self._joint_values):
			userdata.joint_values = self._joint_values
			return 'retrieved'

			
	def on_enter(self, userdata):
		self._sub.enable_buffer(self._topic)
		self._joint_values = [None] * len(self._joints)
		self._joints = userdata.joint_names


	def on_exit(self, userdata):
		self._sub.disable_buffer(self._topic)

# 需要导入模块: from flexbe_core.proxy import ProxySubscriberCached [as 别名]
# 或者: from flexbe_core.proxy.ProxySubscriberCached import has_buffered [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 ProxySubscriberCached [as 别名]
# 或者: from flexbe_core.proxy.ProxySubscriberCached import has_buffered [as 别名]
class MirrorState(EventState):
    '''
    This state will display its possible outcomes as buttons in the GUI and is designed in a way to be created dynamically.
    '''


    def __init__(self, target_name, target_path, given_outcomes, outcome_autonomy):
        '''
        Constructor
        '''
        super(MirrorState, self).__init__(outcomes=given_outcomes)
        self._rate = rospy.Rate(100)
        self._given_outcomes = given_outcomes
        self._outcome_autonomy = outcome_autonomy
        self._target_name = target_name
        self._target_path = target_path
        
        self._outcome_topic = 'flexbe/mirror/outcome'

        self._pub = ProxyPublisher() #{'flexbe/behavior_update': String}
        self._sub = ProxySubscriberCached({self._outcome_topic: UInt8})
        
        
    def execute(self, userdata):
        '''
        Execute this state
        '''
        if JumpableStateMachine.refresh:
            JumpableStateMachine.refresh = False
            self.on_enter(userdata)

        if self._sub.has_buffered(self._outcome_topic):
            msg = self._sub.get_from_buffer(self._outcome_topic)
            if msg.data < len(self._given_outcomes):
                rospy.loginfo("State update: %s > %s", self._target_name, self._given_outcomes[msg.data])
                return self._given_outcomes[msg.data]

        try:
            self._rate.sleep()
        except ROSInterruptException:
            print 'Interrupted mirror sleep.'
    
    
    def on_enter(self, userdata):
        #rospy.loginfo("Mirror entering %s", self._target_path)
        self._pub.publish('flexbe/behavior_update', String("/" + "/".join(self._target_path.split("/")[1:])))

# 需要导入模块: from flexbe_core.proxy import ProxySubscriberCached [as 别名]
# 或者: from flexbe_core.proxy.ProxySubscriberCached import has_buffered [as 别名]
class MonitoringState(smach.State):
    """
    A state to monitor a custom set of conditions.
    For each conditions, an outcome is added or mapped which will be returned if the condition is not met. 
    """

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

        outcomes = kwargs.get('outcomes', [])
        self._outcome_list = list(outcomes)
        self._outcome_list.remove('loopback')

        self.name = None
        self._parent = None
        self._is_controlled = False
        self._force_monitoring = False

        self._monitored_keys = dict()
        self._sent_keys = list()
        self._current_status = None
        
        self.__execute = self.execute
        self.execute = self._monitoring_execute

        self._diagnostics_topic = 'diagnostics_agg'
        self._sub = ProxySubscriberCached()


    def _monitoring_execute(self, *args, **kwargs):
        new_status = None
        had_warning = False
        if (self._force_monitoring or self._is_controlled) and self._sub.has_buffered(self._diagnostics_topic):
            new_status = ""
            diag_msg = self._sub.get_from_buffer(self._diagnostics_topic)
            for status in diag_msg.status:
                if not status.name in self._monitored_keys.keys(): continue
                if status.level == DiagnosticStatus.WARN:
                    had_warning = True
                    if not status.name + "_warn" in self._sent_keys:
                        self._sent_keys.append(status.name + "_warn")
                        Logger.logwarn("%s: %s" % (status.name, status.message))
                if status.level == DiagnosticStatus.ERROR:
                    if not status.name + "_err" in self._sent_keys:
                        self._sent_keys.append(status.name + "_err")
                        Logger.logerr("%s: %s" % (status.name, status.message))
                    new_status = status.name

        if new_status == "":
            self._current_status = None
            new_status = None
            if not had_warning:
                self._sent_keys = list()

        if new_status is None or self._current_status == new_status:
            return self.__execute(*args, **kwargs)

        self._current_status = new_status

        return self._monitored_keys[self._current_status]


    def monitor(self, key, outcome = None):
        oc = outcome if not outcome is None else key
        self._monitored_keys[key] = oc
        if not oc in self._outcomes:
            self.register_outcomes([oc])
            self._outcome_list.append(oc)

    def force_monitoring(self):
        self._force_monitoring = True
        if not self._is_controlled:
            self._sub.subscribe(self._diagnostics_topic, DiagnosticArray)
            self._sub.enable_buffer(self._diagnostics_topic)

    def _enable_ros_control(self):
        self._is_controlled = True
        self._sub.subscribe(self._diagnostics_topic, DiagnosticArray)
        self._sub.enable_buffer(self._diagnostics_topic)

    def _disable_ros_control(self):
        self._is_controlled = False
        self._sub.unsubscribe_topic(self._diagnostics_topic)


    def _get_path(self):
        return self._parent._get_path() + "/" + self.name