Python ProxySubscriberCached.make_persistant方法代码示例

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

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

# 需要导入模块: from flexbe_core.proxy import ProxySubscriberCached [as 别名]
# 或者: from flexbe_core.proxy.ProxySubscriberCached import make_persistant [as 别名]
class AttachObjectState(EventState):
	'''
	Requests a grasp for a template from the template server.

	># template_id 		int 		ID of the template to attach.
	># hand_side 		string 		Hand side to which the template
									should be attached {left, right}

	#> template_pose 	PoseStamped Pose of the attached template in the 
									reference frame of the wrist (r/l_hand)

	<= done 						Template has been attached.
	<= failed 						Failed to attach template.

	'''

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

		# Set up service for attaching object template
		self._service_topic = "/stitch_object_template"
		self._srv = ProxyServiceCaller({
						self._service_topic: SetAttachedObjectTemplate})

		# 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'
			
		return 'done'


	def on_enter(self, userdata):

		self._failed = False

		# The frame ID to which the object template will be attached to
		if userdata.hand_side == 'left':
			frame_id = "l_hand"
		elif userdata.hand_side == 'right':
			frame_id = "r_hand"
		else:
			Logger.logwarn('Unexpected value of hand side: %s Expected {left, right}' % str(userdata.hand_side))
			self._failed = True
			return

		# Get the current wrist pose, which is required by the stitching service
		try:
			wrist_pose_topic   = self._wrist_pose_topics[userdata.hand_side]
			current_wrist_pose = self._sub.get_last_msg(wrist_pose_topic)
			# Set the PoseStamped message's frame ID to the one
			# that the object template should be attached to:
			current_wrist_pose.header.frame_id = frame_id
		except Exception as e:
			Logger.logwarn('Failed to get the latest hand pose:\n%s' % str(e))
			self._failed = True
			return

		# Send the stiching request and write response to userdata
		try:
			request = SetAttachedObjectTemplateRequest(template_id = userdata.template_id,
													   pose = current_wrist_pose)
			self._srv_result = self._srv.call(self._service_topic, request)
			# The result contains the template's pose, mass, and center of mass
			userdata.template_pose = self._srv_result.template_pose
		except Exception as e:
			Logger.logwarn('Failed to send service call:\n%s' % str(e))
			self._failed = True
			return

# 需要导入模块: from flexbe_core.proxy import ProxySubscriberCached [as 别名]
# 或者: from flexbe_core.proxy.ProxySubscriberCached import make_persistant [as 别名]
class ATLAScheckoutSM(Behavior):
	'''
	A behavior meant to be run before high voltage is provided to ATLAS. It guides the robot through BDI's calibration, initial control mode changes, Praying Mantis calibration, and visual calibration check.
	'''


	def __init__(self):
		super(ATLAScheckoutSM, self).__init__()
		self.name = 'ATLAS checkout'

		# parameters of this behavior

		# references to used behaviors
		self.add_behavior(PrayingMantisCalibrationSM, 'Praying Mantis Calibration')

		# Additional initialization code can be added inside the following tags
		# [MANUAL_INIT]

		self._status_topic = '/flor/controller/robot_status'
		self._sub =  ProxySubscriberCached({self._status_topic: FlorRobotStatus})
		self._sub.make_persistant(self._status_topic)
		
		# [/MANUAL_INIT]

		# Behavior comments:



	def create(self):
		# x:1125 y:169, x:430 y:273
		_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
		_state_machine.userdata.left = 'left'
		_state_machine.userdata.right = 'right'
		_state_machine.userdata.none = None

		# Additional creation code can be added inside the following tags
		# [MANUAL_CREATE]
		
		# [/MANUAL_CREATE]

		# x:728 y:81, x:318 y:310
		_sm_confirm_calibration_0 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['left', 'right'])

		with _sm_confirm_calibration_0:
			# x:68 y:71
			OperatableStateMachine.add('Go_to_MANIPULATE',
										ChangeControlModeActionState(target_mode=ChangeControlModeActionState.MANIPULATE),
										transitions={'changed': 'Look_Slightly_Down', 'failed': 'failed'},
										autonomy={'changed': Autonomy.Low, 'failed': Autonomy.Low})

			# x:75 y:301
			OperatableStateMachine.add('Check_Left_Arm',
										MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.CALIBRATE_ARMS, vel_scaling=0.2, ignore_collisions=False, link_paddings={}, is_cartesian=False),
										transitions={'done': 'Bring_Left_Arm_Down', 'failed': 'failed'},
										autonomy={'done': Autonomy.Full, 'failed': Autonomy.Low},
										remapping={'side': 'left'})

			# x:76 y:394
			OperatableStateMachine.add('Bring_Left_Arm_Down',
										MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.SINGLE_ARM_STAND, vel_scaling=0.2, ignore_collisions=False, link_paddings={}, is_cartesian=False),
										transitions={'done': 'Check_Right_Arm', 'failed': 'failed'},
										autonomy={'done': Autonomy.High, 'failed': Autonomy.Low},
										remapping={'side': 'left'})

			# x:426 y:393
			OperatableStateMachine.add('Check_Right_Arm',
										MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.CALIBRATE_ARMS, vel_scaling=0.2, ignore_collisions=False, link_paddings={}, is_cartesian=False),
										transitions={'done': 'Bring_Right_Arm_Down', 'failed': 'failed'},
										autonomy={'done': Autonomy.Full, 'failed': Autonomy.Low},
										remapping={'side': 'right'})

			# x:426 y:301
			OperatableStateMachine.add('Bring_Right_Arm_Down',
										MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.SINGLE_ARM_STAND, vel_scaling=0.2, ignore_collisions=False, link_paddings={}, is_cartesian=False),
										transitions={'done': 'Look_Straight', 'failed': 'failed'},
										autonomy={'done': Autonomy.High, 'failed': Autonomy.Low},
										remapping={'side': 'right'})

			# x:415 y:75
			OperatableStateMachine.add('Go_to_STAND',
										ChangeControlModeActionState(target_mode=ChangeControlModeActionState.STAND),
										transitions={'changed': 'finished', 'failed': 'failed'},
										autonomy={'changed': Autonomy.Off, 'failed': Autonomy.Low})

			# x:81 y:185
			OperatableStateMachine.add('Look_Slightly_Down',
										TiltHeadState(desired_tilt=TiltHeadState.DOWN_30),
										transitions={'done': 'Check_Left_Arm', 'failed': 'failed'},
										autonomy={'done': Autonomy.Off, 'failed': Autonomy.Low})

			# x:449 y:178
			OperatableStateMachine.add('Look_Straight',
										TiltHeadState(desired_tilt=TiltHeadState.STRAIGHT),
										transitions={'done': 'Go_to_STAND', 'failed': 'failed'},
										autonomy={'done': Autonomy.Off, 'failed': Autonomy.Low})


		# x:748 y:57, x:306 y:173
		_sm_checks_and_bdi_calibration_1 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['none'])

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