Python ProxyPublisher.publish方法代码示例

# 需要导入模块: from flexbe_core.proxy import ProxyPublisher [as 别名]
# 或者: from flexbe_core.proxy.ProxyPublisher import publish [as 别名]
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'		

# 需要导入模块: from flexbe_core.proxy import ProxyPublisher [as 别名]
# 或者: from flexbe_core.proxy.ProxyPublisher import publish [as 别名]
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))

# 需要导入模块: from flexbe_core.proxy import ProxyPublisher [as 别名]
# 或者: from flexbe_core.proxy.ProxyPublisher import publish [as 别名]
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)

# 需要导入模块: from flexbe_core.proxy import ProxyPublisher [as 别名]
# 或者: from flexbe_core.proxy.ProxyPublisher import publish [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 publish [as 别名]
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

# 需要导入模块: from flexbe_core.proxy import ProxyPublisher [as 别名]
# 或者: from flexbe_core.proxy.ProxyPublisher import publish [as 别名]
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

# 需要导入模块: from flexbe_core.proxy import ProxyPublisher [as 别名]
# 或者: from flexbe_core.proxy.ProxyPublisher import publish [as 别名]
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

# 需要导入模块: from flexbe_core.proxy import ProxyPublisher [as 别名]
# 或者: from flexbe_core.proxy.ProxyPublisher import publish [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 publish [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 ProxyPublisher [as 别名]
# 或者: from flexbe_core.proxy.ProxyPublisher import publish [as 别名]
class confirm_victim(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(confirm_victim, self).__init__(outcomes=["succeeded"], input_keys=["task_details_task_id"])

        self._topicVictimReached = "victimReached"
        self._pub = ProxyPublisher({self._topicVictimReached: VictimAnswer})

    def execute(self, userdata):
        answer = VictimAnswer()
        answer.task_id = userdata.task_details_task_id
        answer.answer = VictimAnswer.CONFIRM
        self._pub.publish(self._topicVictimReached, answer)

        Logger.loginfo("Victim Found, id = " + str(userdata.task_details_task_id))

        return "succeeded"

    def on_enter(self, userdata):
        pass

    def on_exit(self, userdata):

        pass  # Nothing to do in this example.

    def on_start(self):
        pass

    def on_stop(self):

        pass  # Nothing to do in this example.

# 需要导入模块: from flexbe_core.proxy import ProxyPublisher [as 别名]
# 或者: from flexbe_core.proxy.ProxyPublisher import publish [as 别名]
class SendToOperatorState(EventState):
	'''
	Sends the provided data to the operator using a generic serialized topic.

	># data 	object 	The data to be sent to the operator.
						You should be aware of required bandwidth consumption.

	<= done 			Data has been sent to onboard.
						This is no guarantee that it really has been received.
	<= no_connection 	Unable to send the data.

	'''

	def __init__(self):
		'''Constructor'''
		super(SendToOperatorState, self).__init__(outcomes = ['done', 'no_connection'],
														input_keys = ['data'])

		self._data_topic = "/flexbe/data_to_ocs"
		self._pub = ProxyPublisher({self._data_topic: String})

		self._failed = False


	def execute(self, userdata):
		if self._failed:
			return 'no_connection'

		return 'done'


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

		try:
			self._pub.publish(self._data_topic, String(pickle.dumps(userdata.data)))
		
		except Exception as e:
			Logger.logwarn('Failed to send data to OCS:\n%s' % str(e))
			self._failed = True

# 需要导入模块: from flexbe_core.proxy import ProxyPublisher [as 别名]
# 或者: from flexbe_core.proxy.ProxyPublisher import publish [as 别名]
class LookAtTargetState(EventState):
	'''
	A state that can look at any link target, e.g. one of the hands or a template.
	
	># frame 	string 		Frame to be tracked, pass None to look straight.

	<= done 				Command has been sent to head control.
	
	'''
	
	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})

		
	def execute(self, userdata):
		'''Execute this state'''
		
		return 'done'
		

	
	def on_enter(self, userdata):
		'''Entering the state.'''
		command = HeadControlCommand()

		if userdata.frame is not None:
			command.motion_type = HeadControlCommand.TRACK_FRAME
			command.tracking_frame = userdata.frame
		else:
			command.motion_type = HeadControlCommand.LOOK_STRAIGHT

		self._pub.publish(self._head_control_topic, command)

# 需要导入模块: from flexbe_core.proxy import ProxyPublisher [as 别名]
# 或者: from flexbe_core.proxy.ProxyPublisher import publish [as 别名]

#.........这里部分代码省略.........
            thread.daemon = True
            thread.start()
        else:
            thread = threading.Thread(target=self._stop_mirror, args=[msg])
            thread.daemon = True
            thread.start()


    def _start_mirror(self, msg):
        rate = rospy.Rate(10)
        while self._stopping:
            rate.sleep()

        if self._running:
            rospy.logwarn('Tried to start mirror while it is already running, will ignore.')
            return

        if len(msg.args) > 0:
            self._starting_path = "/" + msg.args[0][1:].replace("/", "_mirror/") + "_mirror"

        self._active_id = msg.behavior_id

        while self._sm is None and len(self._struct_buffer) > 0:
            struct = self._struct_buffer[0]
            self._struct_buffer = self._struct_buffer[1:]
            if struct.behavior_id == self._active_id:
                self._mirror_state_machine(struct)
                rospy.loginfo('Mirror built.')
            else:
                rospy.logwarn('Discarded mismatching buffered structure for ID %s' % str(struct.behavior_id))

        if self._sm is None:
            rospy.logwarn('Missing correct mirror structure, requesting...')
            rospy.sleep(0.2) # no clean way to wait for publisher to be ready...
            self._pub.publish('flexbe/request_mirror_structure', Int32(msg.behavior_id))
            self._active_id = msg.behavior_id
            return

        self._execute_mirror()


    def _stop_mirror(self, msg):
        self._stopping = True
        if self._sm is not None and self._running:
            if msg is not None and msg.code == BEStatus.FINISHED:
                rospy.loginfo('Onboard behavior finished successfully.')
                self._pub.publish('flexbe/behavior_update', String())
            elif msg is not None and msg.code == BEStatus.SWITCHING:
                self._starting_path = None
                rospy.loginfo('Onboard performing behavior switch.')
            elif msg is not None and msg.code == BEStatus.READY:
                rospy.loginfo('Onboard engine just started, stopping currently running mirror.')
                self._pub.publish('flexbe/behavior_update', String())
            elif msg is not None:
                rospy.logwarn('Onboard behavior failed!')
                self._pub.publish('flexbe/behavior_update', String())

            PreemptableState.preempt = True
            rate = rospy.Rate(10)
            while self._running:
                rate.sleep()
        else:
            rospy.loginfo('No onboard behavior is active.')

        self._active_id = 0
        self._sm = None

# 需要导入模块: from flexbe_core.proxy import ProxyPublisher [as 别名]
# 或者: from flexbe_core.proxy.ProxyPublisher import publish [as 别名]
class Send_Request(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(Send_Request, self).__init__(outcomes = ['succeeded'], input_keys = ['pose','params','frameId'])

		#self.userdata.goalId = 'none'
       	 	#self.userdata.frameId = frameId
        	
		useMoveBase=True

        	topic = 'move_base/observe' if useMoveBase else 'controller/goal'
        	self._topic_move_base_goal = topic
		self._pub = ProxyPublisher({self._topic_move_base_goal: MoveBaseActionGoal})
        	

	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.

		##if rospy.Time.now() - self._start_time < self._target_time:
		return 'succeeded' # 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.

		# The following code is just for illustrating how the behavior logger works.
		# Text logged by the behavior logger is sent to the operator and displayed in the GUI.

		##time_to_wait = rospy.Time.now() - self._start_time - self._target_time

		##if time_to_wait > 0:
			##Logger.loginfo('Need to wait for %.1f seconds.' % time_to_wait)

		goal = MoveBaseActionGoal()
       		goal.header.stamp = Time.now()
        	goal.header.frame_id = userdata.frameId
        
        	goal.goal.target_pose.pose.position = userdata.pose.position
        	goal.goal.distance = userdata.params['distance']
        
        	# "straighten up" given orientation
        	yaw = euler_from_quaternion([userdata.pose.orientation.x, userdata.pose.orientation.y, userdata.pose.orientation.z, 		self.pose.orientation.w])[2]
        	goal.goal.target_pose.pose.orientation.x = 0
        	goal.goal.target_pose.pose.orientation.y = 0
        	goal.goal.target_pose.pose.orientation.z = math.sin(yaw/2)
        	goal.goal.target_pose.pose.orientation.w = math.cos(yaw/2)
        	goal.goal.target_pose.header.frame_id = userdata.frameId
        
        	goal.goal_id.id = 'driveTo' + str(goal.header.stamp.secs) + '.' + str(goal.header.stamp.nsecs)
        	goal.goal_id.stamp = goal.header.stamp

        	userdata.goalId = goal.goal_id.id
	      	
		self._pub.publish(self._topic_move_base_goal, goal)
        
        	return 'succeeded'
		


	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 # Nothing to do in this example.


	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.

		# In this example, we use this event to set the correct start time.
		##self._start_time = rospy.Time.now()

		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 # Nothing to do in this example.

# 需要导入模块: from flexbe_core.proxy import ProxyPublisher [as 别名]
# 或者: from flexbe_core.proxy.ProxyPublisher import publish [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)
#.........这里部分代码省略.........