Python ProxyActionClient.setupClient方法代码示例

# 需要导入模块: from flexbe_core.proxy import ProxyActionClient [as 别名]
# 或者: from flexbe_core.proxy.ProxyActionClient import setupClient [as 别名]
class ExecuteTrajectoryBothArmsState(EventState):
	'''
	Executes trajectory(ies) passed from userdata.

	># trajectories JointTrajectory{}	A dictionary where the keys are ['left_arm', 'right_arm'] and each has a trajectory as the value.

	<= done 							The trajectories were successfully executed.
	<= failed 							Failed to load trajectory.

	'''

	def __init__(self, controllers = ["left_arm_traj_controller", "right_arm_traj_controller"]):
		'''Constructor'''
		
		super(ExecuteTrajectoryBothArmsState, self).__init__(outcomes = ['done', 'failed'],
															 input_keys = ['trajectories'])

		self._controllers = controllers
		self._controllers = ["left_arm_traj_controller", "right_arm_traj_controller"] if not(controllers) else controllers

		self._client = ProxyActionClient()
		
		self._client_topics = dict()
		self._active_topics = list()
		
		for controller in self._controllers:
			if "left_arm" in controller:
				action_topic_left  = "/trajectory_controllers/" + controller + "/follow_joint_trajectory"
				self._client.setupClient(action_topic_left, FollowJointTrajectoryAction)
				self._client_topics['left_arm'] = action_topic_left
			elif "right_arm" in controller:
				action_topic_right = "/trajectory_controllers/" + controller + "/follow_joint_trajectory"
				self._client.setupClient(action_topic_right, FollowJointTrajectoryAction)
				self._client_topics['right_arm'] = action_topic_right
			else:
				Logger.logwarn('The controller is neither a left nor a right arm trajectory controller!? %s' % str(controller))

		self._failed = False


	def execute(self, userdata):
		'''Code to be executed while SM is in this state.'''

		if self._failed:
			return 'failed'

		results_ready = [self._client.has_result(topic) for topic in self._active_topics]

		if all(results_ready):

			results = [self._client.get_result(topic) for topic in self._active_topics]

			if any([r == None for r in results]):
				Logger.logwarn('One of the action calls returned None as result.')
				self._failed = True
				return 'failed'

			if all(r.error_code is FollowJointTrajectoryResult.SUCCESSFUL for r in results):
				return 'done'
			else:
				for r in results:
					Logger.logwarn('Joint trajectory request result: (%d) %s' % (r.error_code, r.error_string))
				Logger.logwarn('One of the requests failed to execute.')
				
				self._failed = True
				return 'failed'


	def on_enter(self, userdata):
		'''Upon entering the state, write trajectory goal(s) to action server(s)'''

		self._failed = False

		Logger.loginfo('Received trajectory for %s arm(s).' % userdata.trajectories.keys())
		Logger.loginfo('Using %s' % str(self._controllers))

		# In execute, only check clients that have sent goals/trajectories
		self._active_topics = list()				

		# Setup client and send goal to server for execution
		try:
			for arm, traj in userdata.trajectories.iteritems():

				# Create goal message
				action_goal = FollowJointTrajectoryGoal()
				action_goal.trajectory = traj
				action_goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(1.0)

				topic = self._client_topics[arm]
				self._client.send_goal(topic, action_goal)
				self._active_topics.append(topic)
				
				Logger.loginfo('Goal message sent for %s trajectory' % str(arm))
	
		except Exception as e:
			Logger.logwarn('Failed to send follow joint trajectory action goal(s):\n%s' % str(e))
			self._failed = True

# 需要导入模块: from flexbe_core.proxy import ProxyActionClient [as 别名]
# 或者: from flexbe_core.proxy.ProxyActionClient import setupClient [as 别名]
class ExecuteTrajectoryWholeBodyState(EventState):
	'''
	Executes trajectory(ies) passed from userdata.

	># trajectories JointTrajectory{}	A dictionary where the keys are ['left_arm', 'right_arm', 'left_leg', 'right_leg', 'torso'] and each has a trajectory as the value.

	<= done 							The trajectories were successfully executed.
	<= failed 							Failed to load trajectory.

	'''
	
	CONTROLLER_LEFT_ARM = "left_arm_traj_controller"
	CONTROLLER_RIGHT_ARM = "right_arm_traj_controller"
	CONTROLLER_PELVIS = "pelvis_traj_controller"
	CONTROLLER_TORSO = "torso_traj_controller"
	CONTROLLER_LEFT_LEG = "left_leg_traj_controller"
	CONTROLLER_RIGHT_LEG = "right_leg_traj_controller"
	CONTROLLER_NECK = "neck_traj_controller"
	

	def __init__(self, controllers = ["left_arm_traj_controller", "right_arm_traj_controller", "torso_traj_controller", "left_leg_traj_controller", "right_leg_traj_controller"]):
		'''Constructor'''
		
		super(ExecuteTrajectoryWholeBodyState, self).__init__(outcomes = ['done', 'failed'],
															 input_keys = ['trajectories'])

		if not rospy.has_param("behavior/joint_controllers_name"):
			Logger.logerr("Need to specify parameter behavior/joint_controllers_name at the parameter server")
			return

		controller_namespace = rospy.get_param("behavior/joint_controllers_name")

		self._controllers = controllers
		self._controllers = ["torso_traj_controller", "left_leg_traj_controller", "right_leg_traj_controller", "left_arm_traj_controller", "right_arm_traj_controller"] if not(controllers) else controllers

		self._client = ProxyActionClient()
		
		self._client_topics = dict()
		self._active_topics = list()
		
		for controller in self._controllers:
			if "left_arm" in controller:
				action_topic_left  = "/" + controller_namespace + "/" + controller + "/follow_joint_trajectory"
				self._client.setupClient(action_topic_left, FollowJointTrajectoryAction)
				self._client_topics['left_arm'] = action_topic_left
			elif "right_arm" in controller:
				action_topic_right = "/" + controller_namespace + "/" + controller + "/follow_joint_trajectory"
				self._client.setupClient(action_topic_right, FollowJointTrajectoryAction)
				self._client_topics['right_arm'] = action_topic_right
			elif "left_leg" in controller:
				action_topic_left  = "/" + controller_namespace + "/" + controller + "/follow_joint_trajectory"
				self._client.setupClient(action_topic_left, FollowJointTrajectoryAction)
				self._client_topics['left_leg'] = action_topic_left
			elif "right_leg" in controller:
				action_topic_right = "/" + controller_namespace + "/" + controller + "/follow_joint_trajectory"
				self._client.setupClient(action_topic_right, FollowJointTrajectoryAction)
				self._client_topics['right_leg'] = action_topic_right
			elif "torso" in controller:
				action_topic_right = "/" + controller_namespace + "/" + controller + "/follow_joint_trajectory"
				self._client.setupClient(action_topic_right, FollowJointTrajectoryAction)
				self._client_topics['torso'] = action_topic_right
			else:
				Logger.logwarn('ExecuteTrajectoryWholeBodyState:The controller is neither an arm, leg or torso trajectory controller!? %s' % str(controller))
		print self._client_topics
		print self._client
		self._failed = False


	def execute(self, userdata):
		'''Code to be executed while SM is in this state.'''

		if self._failed:
			return 'failed'
		results_ready = [self._client.has_result(topic) for topic in self._active_topics]
		if all(results_ready):
			results = [self._client.get_result(topic) for topic in self._active_topics]
			if all(r.error_code is FollowJointTrajectoryResult.SUCCESSFUL for r in results):
				return 'done'
			else:
				for r in results:
					Logger.logwarn('ExecuteTrajectoryWholeBodyState: Joint trajectory request result: (%d) %s' % (r.error_code, r.error_string))
				Logger.logwarn('ExecuteTrajectoryWholeBodyState: One of the requests failed to execute.')
				
				self._failed = True
				return 'failed'


	def on_enter(self, userdata):
		'''Upon entering the state, write trajectory goal(s) to action server(s)'''

		self._failed = False
		Logger.loginfo('ExecuteTrajectoryWholeBodyState: Received trajectory for %s.' % userdata.trajectories.keys())

		# In execute, only check clients that have sent goals/trajectories
		self._active_topics = list()				

		# Setup client and send goal to server for execution
		try:
			for chain, traj in userdata.trajectories.iteritems():
			
#.........这里部分代码省略.........

# 需要导入模块: from flexbe_core.proxy import ProxyActionClient [as 别名]
# 或者: from flexbe_core.proxy.ProxyActionClient import setupClient [as 别名]
class HandTrajectoryState(EventState):
	'''
	Executes a given hand trajectory, i.e., a request to open or close the fingers.

	-- hand_type			string 			Type of hand (e.g. 'robotiq')

	># finger_trajectory 	JointTrajectory	A single joint trajectory for the hand joints.
	># hand_side			string 			Which hand side the trajectory refers to (e.g. 'left')

	<= done 								The trajectory was successfully executed.
	<= failed 								Failed to execute trajectory.

	'''

	def __init__(self, hand_type):
		'''Constructor'''
		super(HandTrajectoryState, self).__init__(outcomes=['done', 'failed'],
												  input_keys=['finger_trajectory', 'hand_side'])

		if not rospy.has_param("behavior/hand_controllers_name"):
			Logger.logerr("Need to specify parameter behavior/hand_controllers_name at the parameter server")
			return

		controller_namespace = rospy.get_param("behavior/hand_controllers_name")

		if not rospy.has_param("behavior/hand_type_prefix"):
			Logger.logerr("Need to specify parameter behavior/hand_type_prefix at the parameter server")
			return

		hand_type_prefix = rospy.get_param("behavior/hand_type_prefix")

		# Determine which hand types and sides have been sourced
		self._hands_in_use = list()

		LEFT_HAND  = os.environ[hand_type_prefix + 'LEFT_HAND_TYPE']
		RIGHT_HAND = os.environ[hand_type_prefix + 'RIGHT_HAND_TYPE']

		if LEFT_HAND == 'l_' + hand_type:
			self._hands_in_use.append('left')
		if RIGHT_HAND == 'r_' + hand_type:
			self._hands_in_use.append('right')

		if len(self._hands_in_use) == 0:
			Logger.logerr('No %s hands seem to be in use:\nLEFT_HAND = %s\nRIGHT_HAND = %s' % (hand_type, LEFT_HAND, RIGHT_HAND))

		# Initialize the action clients corresponding to the hands in use
		self._client_topics = dict()
		self._active_topic = None
		self._client = ProxyActionClient()
		
		for hand_side in self._hands_in_use:
			action_topic = ("/%s/%s_hand_traj_controller/follow_joint_trajectory" % (controller_namespace, hand_side))
			self._client.setupClient(action_topic, FollowJointTrajectoryAction)
			self._client_topics[hand_side] = action_topic

		self._failed = False
	
	def execute(self, userdata):
		'''During execution of the state, keep checking for action servers response'''

		if self._failed:
			return 'failed'

		if self._client.has_result(self._active_topic):
			result = self._client.get_result(self._active_topic)
			# Logger.loginfo('Action server says: %s' % result.error_code)
			
			if result.error_code == FollowJointTrajectoryResult.SUCCESSFUL:
				return 'done'
			else:
				Logger.logwarn('Hand trajectory request failed to execute: %s (%d)' % (result.error_string, result.error_code))
				
				self._failed = True
				return 'failed'


	def on_enter(self, userdata):
		'''Upon entering the state, create and send the action goal message'''

		self._failed = False

		if userdata.hand_side not in self._hands_in_use:
			Logger.logerr('Hand side from userdata (%s) does not match hands in use: %s' % (userdata.hand_side, self._hands_in_use))
			self._failed = True
			return

		# Create goal message
		goal = FollowJointTrajectoryGoal()
		goal.trajectory = userdata.finger_trajectory

		# Send goal to action server for execution
		try: 
			self._active_topic = self._client_topics[userdata.hand_side]
			self._client.send_goal(self._active_topic, goal)
		except Exception as e:
			Logger.logwarn('Failed to send follow (hand) joint trajectory action goal:\n%s' % str(e))
			self._failed = True


	def on_exit(self, userdata):
#.........这里部分代码省略.........