Python proxy.ProxySubscriberCached类代码示例

class CheckCurrentControlModeState(EventState):
	'''
	Implements a state where the robot checks its current control mode.

	-- target_mode 	enum	The control mode to check for being the current one (e.g. 3 for stand, 6 for manipulate).
							The state's class variables can also be used (e.g. CheckCurrentControlModeState.STAND).
	-- wait 		bool	Whether to check once and return (False), or wait for the control mode to change (True).

	#> control_mode enum	The current control mode when the state returned.

	<= correct				Indicates that the current control mode is the target/expected one.
	<= incorrect			Indicates that the current control mode is not the target/expected one.

	'''

	NONE = 0
	FREEZE = 1
	STAND_PREP = 2
	STAND = 3
	STAND_MANIPULATE = 3
	WALK = 4
	STEP = 5
	MANIPULATE = 6
	USER = 7
	CALIBRATE = 8
	SOFT_STOP = 9


	def __init__(self, target_mode, wait = False):
		'''
		Constructor
		'''
		super(CheckCurrentControlModeState, self).__init__(outcomes=['correct', 'incorrect'],
														   output_keys=['control_mode'])
		
		self._status_topic = '/flor/controller/mode'
		
		self._sub = ProxySubscriberCached({self._status_topic: VigirAtlasControlMode})
		
		self._sub.make_persistant(self._status_topic)
		
		self._target_mode = target_mode
		self._wait = wait
		
		
	def execute(self, userdata):
		'''
		Execute this state
		'''
		if self._sub.has_msg(self._status_topic):
			msg = self._sub.get_last_msg(self._status_topic)
			userdata.control_mode = msg.bdi_current_behavior
			if msg.bdi_current_behavior == self._target_mode:
				return 'correct'
			elif not self._wait:
				return 'incorrect'
		
	def on_enter(self, userdata):
		if self._wait:
			Logger.loghint("Waiting for %s" % str(self._target_mode))

class ShowPictureWebinterfaceState(EventState):
    '''
    Displays the picture in a web interface

    ># Image    Image       The received Image
    <= done                 Displaying the Picture
    '''

    def __init__(self):
        super(ShowPictureWebinterfaceState, self).__init__(outcomes = ['tweet', 'forget'],
                                                            input_keys = ['image_name'])

        self._pub_topic = '/webinterface/display_picture'
        self._pub = ProxyPublisher({self._pub_topic: String})

        self._sub_topic = '/webinterface/dialog_feedback'
        self._sub = ProxySubscriberCached({self._sub_topic: String})
                

    def execute(self, userdata):
        if self._sub.has_msg(self._sub_topic):
            msg = self._sub.get_last_msg(self._sub_topic)

            if msg.data == 'yes':
		print 'show_picture_webinterface_state, returning tweet'
                return 'tweet'
            else:
		print 'show_picture_webinterface_state, returning forget'
                return 'forget'

    def on_enter(self,userdata):
        self._sub.remove_last_msg(self._sub_topic)
        self._pub.publish(self._pub_topic, String(userdata.image_name))

class StorePointcloudState(EventState):
	'''
	Stores the latest pointcloud of the given topic.

	-- topic 		string 			The topic on which to listen for the pointcloud.

	#> pointcloud 	PointCloud2		The received pointcloud.

	<= done 			Pointcloud has been received and stored.

	'''

	def __init__(self, topic):
		super(StorePointcloudState, self).__init__(outcomes = ['done'],
													output_keys = ['pointcloud'])

		self._sub = ProxySubscriberCached({topic: PointCloud2})

		self._pcl_topic = topic


	def execute(self, userdata):
		
		if self._sub.has_msg(self._pcl_topic):
			userdata.pointcloud = self._sub.get_last_msg(self._pcl_topic)
			return 'done'

class GetCameraImageState(EventState):
	'''
	Grabs the most recent camera image.

	#> camera_img 	Image 	The current color image of the left camera.

	<= done 				Image data is available.

	'''

	def __init__(self):
		'''Constructor'''
		super(GetCameraImageState, self).__init__(outcomes = ['done'],
														output_keys = ['camera_img'])

		self._img_topic = "/multisense/camera/left/image_rect_color"
		self._sub = ProxySubscriberCached({self._img_topic: Image})


	def execute(self, userdata):

		if self._sub.has_msg(self._img_topic):
			userdata.camera_img = self._sub.get_last_msg(self._img_topic)
			return 'done'


	def on_enter(self, userdata):
		pass

class DetectPersonState(EventState):
    """
	Detects the nearest person and provides their pose.

	-- wait_timeout	float 			Time (seconds) to wait for a person before giving up.

	#> person_pose 	PoseStamped 	Pose of the nearest person if one is detected, else None.

	<= detected 		Detected a person.
	<= not_detected		No person detected, but time ran out.

	"""

    def __init__(self, wait_timeout):
        super(DetectPersonState, self).__init__(outcomes=["detected", "not_detected"], output_keys=["person_pose"])

        self._wait_timeout = rospy.Duration(wait_timeout)

        self._topic = "/people_tracker/pose"
        self._sub = ProxySubscriberCached({self._topic: PoseStamped})

        self._start_waiting_time = None

    def execute(self, userdata):
        if rospy.Time.now() > self._start_waiting_time + self._wait_timeout:
            userdata.person_pose = None
            return "not_detected"

        if self._sub.has_msg(self._topic):
            userdata.person_pose = self._sub.get_last_msg(self._topic)
            return "detected"

    def on_enter(self, userdata):
        self._sub.remove_last_msg(self._topic)
        self._start_waiting_time = rospy.Time.now()

class GetLaserscanState(EventState):
    """
	Grabs the most recent laserscan data.

	#> laserscan 	LaserScan 	The current laser scan.

	<= done 					Scanning data is available.

	"""

    def __init__(self):
        """Constructor"""
        super(GetLaserscanState, self).__init__(outcomes=["done"], output_keys=["laserscan"])

        self._scan_topic = "/multisense/lidar_scan"
        self._sub = ProxySubscriberCached({self._scan_topic: LaserScan})

    def execute(self, userdata):

        if self._sub.has_msg(self._scan_topic):
            userdata.laserscan = self._sub.get_last_msg(self._scan_topic)
            return "done"

    def on_enter(self, userdata):
        pass

class PreemptableStateMachine(LoopbackStateMachine):
    """
    A state machine that can be preempted.
    If preempted, the state machine will return the outcome preempted.
    """
    
    _preempted_name = 'preempted'
    
    def __init__(self, *args, **kwargs):
        # add preempted outcome
        if len(args) > 0:
            args[0].append(self._preempted_name)
        else:
            outcomes = kwargs.get('outcomes', [])
            outcomes.append(self._preempted_name)
            kwargs['outcomes'] = outcomes
            
        super(PreemptableStateMachine, self).__init__(*args, **kwargs)

        # always listen to preempt so that the behavior can be stopped even if unsupervised
        self._preempt_topic = 'flexbe/command/preempt'
        self._sub = ProxySubscriberCached({self._preempt_topic: Empty})
        self._sub.set_callback(self._preempt_topic, self._preempt_cb)


    def _preempt_cb(self, msg):
        if not self._is_controlled:
            PreemptableState.preempt = True
            rospy.loginfo("--> Preempted requested while unsupervised")

class MonitorPerceptState(EventState):
	'''
	Monitors the hector worldmodel for percepts
	and triggers a detection event if a percept has a high-enough support.

	-- required_support	float 		Support threshold which needs to be reached before an event is triggered.
	-- percept_class	string 		Class name of percepts to be monitored.
	-- track_percepts 	bool 		Defines if this state should track previously detected percepts.
									Recommended if it is not desired to react multiple times on the same percept.

	#> object_id 		string 		Identifier of the detected object.
	#> object_pose 		PoseStamped Pose of the detected object.
	#> object_data 		float[] 	Data associated with this object.

	<= detected 					Percept is detected.

	'''

	def __init__(self, required_support, percept_class, track_percepts = True):
		'''
		Constructor
		'''
		super(MonitorPerceptState, self).__init__(outcomes=['detected'],
												output_keys=['object_id', 'object_pose', 'object_data'])
		
		self._topic = '/worldmodel/objects'
		self._sub = ProxySubscriberCached({self._topic: ObjectModel})

		self._required_support = required_support
		self._percept_class = percept_class
		self._track_percepts = track_percepts
		self._percepts = list()
			
		
	def execute(self, userdata):
		if self._sub.has_msg(self._topic):
			msg = self._sub.get_last_msg(self._topic)

			objects = filter(lambda obj:
				#obj.state.state == ObjectState.ACTIVE \
				obj.info.class_id == self._percept_class \
				and obj.info.support >= self._required_support \
				and obj.info.object_id not in self._percepts,
				msg.objects
			)

			for obj in objects:
			
				if self._track_percepts:
					self._percepts.append(obj.info.object_id)

				(x,y,z) = (obj.pose.pose.position.x, obj.pose.pose.position.y, obj.pose.pose.position.z)
				Logger.loginfo('Detected %s percept with id: %s\nPosition: (%.1f, %.1f, %.1f)' % (self._percept_class, obj.info.object_id, x, y, z))

				userdata.object_id = obj.info.object_id
				userdata.object_pose = PoseStamped(header=obj.header, pose=obj.pose.pose)
				userdata.object_data = obj.info.data
				return 'detected'

class GetWristPoseState(EventState):
	'''
	Retrieves the current wrist pose from the corresponding ROS topic.

	># hand_side 	string 		Wrist whose pose will be returned (left, right)

	#> wrist_pose 	PoseStamped	The current pose of the left or right wrist

	<= done 					Wrist pose has been retrieved.
	<= failed 					Failed to retrieve wrist pose.

	'''

	def __init__(self):
		'''Constructor'''
		super(GetWristPoseState, self).__init__(outcomes = ['done', 'failed'],
												input_keys = ['hand_side'],
												output_keys = ['wrist_pose'])

		# Set up subscribers for listening to the latest wrist poses
		self._wrist_pose_topics = dict()
		self._wrist_pose_topics['left']  = '/flor/l_arm_current_pose'
		self._wrist_pose_topics['right'] = '/flor/r_arm_current_pose'
		
		self._sub = ProxySubscriberCached({
						self._wrist_pose_topics['left']:  PoseStamped,
						self._wrist_pose_topics['right']: PoseStamped})
		
		self._sub.make_persistant(self._wrist_pose_topics['left'])
		self._sub.make_persistant(self._wrist_pose_topics['right'])

		self._failed = False


	def execute(self, userdata):

		if self._failed:
			return 'failed'
		else:
			return 'done'


	def on_enter(self, userdata):

		self._failed = False

		# Read the current wrist pose and write to userdata
		try:
			wrist_pose_topic = self._wrist_pose_topics[userdata.hand_side]
			wrist_pose = self._sub.get_last_msg(wrist_pose_topic)
			userdata.wrist_pose = wrist_pose
			rospy.loginfo('Retrieved %s wrist pose: \n%s',
						  userdata.hand_side, wrist_pose)
		except Exception as e:
			Logger.logwarn('Failed to get the latest wrist pose:\n%s' % str(e))
			self._failed = True
			return

	def __init__(self, topic):
		super(StorePointcloudState, self).__init__(outcomes = ['done'],
													output_keys = ['pointcloud'])

		self._sub = ProxySubscriberCached({topic: PointCloud2})

		self._pcl_topic = topic

    def __init__(self):
        '''
        Constructor
        '''
        self._sm = None
        
        # set up proxys for sm <--> GUI communication
        # publish topics
        self._pub = ProxyPublisher({'/flexbe/behavior_update': String,
                                    '/flexbe/request_mirror_structure': Int32})
            
        self._running = False
        self._stopping = False
        self._active_id = 0
        self._starting_path = None
        self._current_struct = None

        self._outcome_topic = '/flexbe/mirror/outcome'

        self._struct_buffer = list()
        
        # listen for mirror message
        self._sub = ProxySubscriberCached()
        self._sub.subscribe(self._outcome_topic, UInt8)
        self._sub.enable_buffer(self._outcome_topic)

        self._sub.subscribe('/flexbe/mirror/structure', ContainerStructure, self._mirror_callback)
        self._sub.subscribe('/flexbe/status', BEStatus, self._status_callback)
        self._sub.subscribe('/flexbe/mirror/sync', BehaviorSync, self._sync_callback)
        self._sub.subscribe('/flexbe/mirror/preempt', Empty, self._preempt_callback)

	def __init__(self):
		'''Constructor'''
		super(GetCameraImageState, self).__init__(outcomes = ['done'],
														output_keys = ['camera_img'])

		self._img_topic = "/multisense/camera/left/image_rect_color"
		self._sub = ProxySubscriberCached({self._img_topic: Image})

	def __init__(self):
		super(GetRobotPose, self).__init__(outcomes = ['succeeded'], output_keys = ['pose'])

		self._objectTopic = '/robot_pose'
		self._sub = ProxySubscriberCached({self._objectTopic: PoseStamped})

		self._succeeded = False

    def __init__(self):
        """
        Constructor
        """
        self._sm = None

        smach.set_loggers(
            rospy.logdebug, rospy.logwarn, rospy.logdebug, rospy.logerr  # hide SMACH transition log spamming
        )

        # set up proxys for sm <--> GUI communication
        # publish topics
        self._pub = ProxyPublisher({"flexbe/behavior_update": String, "flexbe/request_mirror_structure": Int32})

        self._running = False
        self._stopping = False
        self._active_id = 0
        self._starting_path = None
        self._current_struct = None

        self._outcome_topic = "flexbe/mirror/outcome"

        self._struct_buffer = list()

        # listen for mirror message
        self._sub = ProxySubscriberCached()
        self._sub.subscribe(self._outcome_topic, UInt8)
        self._sub.enable_buffer(self._outcome_topic)

        self._sub.subscribe("flexbe/mirror/structure", ContainerStructure, self._mirror_callback)
        self._sub.subscribe("flexbe/status", BEStatus, self._status_callback)
        self._sub.subscribe("flexbe/mirror/sync", BehaviorSync, self._sync_callback)
        self._sub.subscribe("flexbe/mirror/preempt", Empty, self._preempt_callback)

	def __init__(self, topic):
		'''Constructor'''
		super(TestSubState, self).__init__(outcomes=['received', 'unavailable'],
										output_keys=['result'])
		self._topic = topic
		self._sub = ProxySubscriberCached({self._topic: String})
		self._msg_counter = 0
		self._timeout = rospy.Time.now() + rospy.Duration(1.5)