Python proxy.ProxyPublisher类代码示例

class complete_task_StartHack(EventState):
	'''
	Example for a state to demonstrate which functionality is available for state implementation.
	This example lets the behavior wait until the given target_time has passed since the behavior has been started.

	-- target_time 	float 	Time which needs to have passed since the behavior started.

	<= continue 			Given time has passed.
	<= failed 				Example for a failure outcome.

	'''

	def __init__(self):
		# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
		super(complete_task_StartHack, self).__init__(outcomes = ['error','done'], input_keys = ['task_details_task_id'])
		
		self._topic_set_task_state = 'taskallocation/set_task_state'
		self._pub = ProxyPublisher({self._topic_set_task_state: SetTaskState})

        	self._task_state_completed = TaskState()
        	self._task_state_completed.state = TaskState.COMPLETED

		
	def execute(self, userdata):

		request = SetTaskStateRequest()
        	request.task_id = userdata.task_details_task_id
        	request.new_state = self._task_state_completed
        
        	try:
			self._pub.publish(self._topic_set_task_state, request)
            		return 'done'
        	except Exception, e:
            		Logger.logwarn('Could not set task state:' + str(e))
            		return 'error'		

class PublishPoseState(EventState):
	"""
	Publishes a pose from userdata so that it can be displayed in rviz.

	-- topic 		string 			Topic to which the pose will be published.

	># pose			PoseStamped		Pose to be published.

	<= done							Pose has been published.

	"""
	
	def __init__(self, topic):
		"""Constructor"""
		super(PublishPoseState, self).__init__(outcomes=['done'],
												input_keys=['pose'])

		self._topic = topic
		self._pub = ProxyPublisher({self._topic: PoseStamped})


	def execute(self, userdata):
		return 'done'
	
	def on_enter(self, userdata):
		self._pub.publish(self._topic, userdata.pose)

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 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)

class PauseExploration(EventState):
	'''
	Example for a state to demonstrate which functionality is available for state implementation.
	This example lets the behavior wait until the given target_time has passed since the behavior has been started.

	-- target_time 	float 	Time which needs to have passed since the behavior started.

	<= continue 			Given time has passed.
	<= failed 				Example for a failure outcome.

	'''

	def __init__(self):
		# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
		super(PauseExploration, self).__init__(outcomes = ['continue', 'pause'])
	
		self._topic_cancel = 'move_base/cancel'
		self._pub = ProxyPublisher({self._topic_cancel: Empty})



	def execute(self, userdata):
		# This method is called periodically while the state is active.
		# Main purpose is to check state conditions and trigger a corresponding outcome.
		# If no outcome is returned, the state will stay active.

		return 'pause' # One of the outcomes declared above.
		

	def on_enter(self, userdata):
		# This method is called when the state becomes active, i.e. a transition from another state to this one is taken.
		# It is primarily used to start actions which are associated with this state.

		self._pub.publish(self._topic_cancel, Empty())


	def on_exit(self, userdata):
		# This method is called when an outcome is returned and another state gets active.
		# It can be used to stop possibly running processes started by on_enter.

		pass


	def on_start(self):
		# This method is called when the behavior is started.
		# If possible, it is generally better to initialize used resources in the constructor
		# because if anything failed, the behavior would not even be started.

		pass


	def on_stop(self):
		# This method is called whenever the behavior stops execution, also if it is cancelled.
		# Use this event to clean up things like claimed resources.

		pass

class RobotStateCommandState(EventState):
	'''
	Publishes a robot state command message.

	-- command 	int 	Command to be sent. Use class variables (e.g. STAND).

	<= done				Successfully published the state command message.
	<= failed			Failed to publish the state command message.

	'''

	START_HYDRAULIC_PRESSURE_OFF = 4 # "fake" start (without hydraulic pressure)
	START_HYDRAULIC_PRESSURE_ON = 6	 # start normally (with hydraulic pressure)
	STOP = 8 # stop the pump
	FREEZE = 16
	STAND = 32
	STAND_PREP = 33
	CALIBRATE = 64 # BIASES
	CALIBRATE_ARMS = 128
	CALIBRATE_ARMS_FREEZE = 144

	def __init__(self, command):
		'''Constructor'''
		super(RobotStateCommandState, self).__init__(outcomes = ['done', 'failed'])

		self._topic = '/flor/controller/robot_state_command'
		self._pub = ProxyPublisher({self._topic: FlorRobotStateCommand})

		self._command = command

		self._failed = False


	def execute(self, userdata):
		if self._failed:
			return 'failed'
		else:
			return 'done'


	def on_enter(self, userdata):
		self._failed = False

		try:
			command_msg = FlorRobotStateCommand(state_command = self._command)
			self._pub.publish(self._topic, command_msg)
		
		except Exception as e:
			Logger.logwarn('Failed to publish the command message:\n%s' % str(e))
			self._failed = True

class VideoLoggingState(EventState):
	"""
	A state that controls video logging.

	-- command 			boolean 	One of the available commands provided as class constants.
	-- no_video 		boolean 	Only create bag files.
	-- no_bags			boolean 	Only record video.

	># experiment_name 	string 		Unique name of the experiment.

	<= done 						Executed the specified command.

	"""
	
	START = True
	STOP  = False

	def __init__(self, command, no_video=False, no_bags=True):
		"""Constructor"""
		super(VideoLoggingState, self).__init__(outcomes=['done'],
												input_keys=['experiment_name', 'description'])

		self._topic = "/vigir_logging"
		self._pub   = ProxyPublisher({self._topic: OCSLogging})

		self._command  = command
		self._no_video = no_video
		self._no_bags  = no_bags

	def execute(self, userdata):
		"""Execute this state"""

		# nothing to check
		return 'done'
	
	def on_enter(self, userdata):
		"""Upon entering the state"""

		try:
			self._pub.publish(self._topic, OCSLogging(run=self._command, no_video=self._no_video, no_bags=self._no_bags, experiment_name=userdata.experiment_name, description=userdata.description))
		except Exception as e:
			Logger.logwarn('Could not send video logging command:\n %s' % str(e))
		
	def on_stop(self):
		"""Stop video logging upon end of execution"""
		
		try:
			self._pub.publish(self._topic, OCSLogging(run=False))
		except Exception as e:
			pass

    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, topic):
		"""Constructor"""
		super(PublishPoseState, self).__init__(outcomes=['done'],
												input_keys=['pose'])

		self._topic = topic
		self._pub = ProxyPublisher({self._topic: PoseStamped})

    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)

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)

	def __init__(self):
		'''Constructor'''
		super(LookAtTargetState, self).__init__(outcomes=['done'],
												input_keys=['frame'])
		
		self._head_control_topic = '/thor_mang/head_control_mode'

		self._pub = ProxyPublisher({self._head_control_topic: HeadControlCommand})

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:])))

	def __init__(self):
		# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
		super(Drive_to_new, self).__init__(outcomes = ['succeeded'], input_keys = ['pose'])



        	self._topic = '/move_base/simple_goal'
		self._pub = ProxyPublisher({self._topic: PoseStamped})

    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})