Python ProxyActionClient.get_result方法代码示例

# 需要导入模块: from flexbe_core.proxy import ProxyActionClient [as 别名]
# 或者: from flexbe_core.proxy.ProxyActionClient import get_result [as 别名]
class MetricSweepState(EventState):
	'''
	Robot performs a metric sweep at its current location.

	-- sweep_type	string 	Type of the sweep to do (select one of the provided options).

	<= sweeped 		Sweep has been completed.
	<= failed		There has been an error when sweeping.

	'''

	COMPLETE = 'complete'
	MEDIUM = 'medium'
	SHORT = 'short'
	SHORTEST = 'shortest'

	def __init__(self, sweep_type):
		super(MetricSweepState, self).__init__(outcomes = ['sweeped', 'failed'])

		self._sweep_type = sweep_type

		self._topic = '/do_sweep'
		self._client = ProxyActionClient({self._topic: SweepAction})

		self._error = False


	def execute(self, userdata):
		if self._error:
			return 'failed'

		if self._client.has_result(self._topic):
			result = self._client.get_result(self._topic)

			if result.success:
				return 'sweeped'
			else:
				return 'failed'

		
	def on_enter(self, userdata):
		goal = SweepGoal()
		goal.type = self._sweep_type

		self._error = False
		try:
			self._client.send_goal(self._topic, goal)
		except Exception as e:
			Logger.logwarn('Failed to send the Sweep command:\n%s' % str(e))
			self._error = True


	def on_exit(self, userdata):
		if not self._client.has_result(self._topic):
			self._client.cancel(self._topic)
			Logger.loginfo('Cancelled active action goal.')

# 需要导入模块: from flexbe_core.proxy import ProxyActionClient [as 别名]
# 或者: from flexbe_core.proxy.ProxyActionClient import get_result [as 别名]
class GripperState(EventState):
	'''
	Open or close the gripper.

	-- close_fraction 	float 	OPEN, CLOSE or any value in [0,1].
	-- duration 		float 	Time (sec) for executing the motion.

	<= done 					Trajectory has successfully finished its execution.
	<= failed 					Failed to execute trajectory.

	'''
	OPEN = 0
	CLOSE = 1

	def __init__(self, action, duration = 1.0):
		'''
		Constructor
		'''
		super(GripperState, self).__init__(outcomes=['done', 'failed'])

		self._action_topic = "/gripper_control/gripper_grasp_traj_controller/follow_joint_trajectory"
		self._client = ProxyActionClient({self._action_topic: FollowJointTrajectoryAction})

		self._failed = False
		self._duration = duration
		self._target_joint_value = action * 1.89


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

		if self._client.has_result(self._action_topic):
			result = self._client.get_result(self._action_topic)
			if result:
				if result.error_code == FollowJointTrajectoryResult.SUCCESSFUL:
					return 'done'
				else:
					Logger.logwarn('Joint trajectory request failed to execute (%d). Reason: %s' % (result.error_code, result.error_string))
					self._failed = True
					return 'failed'
			else:
				Logger.logwarn('Wait for result returned True even though the result is %s' % str(result))
				self._failed = True
				return 'failed'


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

		# create point
		endpoint = JointTrajectoryPoint()
		endpoint.positions.append(self._target_joint_value)
		endpoint.time_from_start = rospy.Duration(0.2 + self._duration)

		# create goal
		goal = FollowJointTrajectoryGoal()
		goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2)
		goal.trajectory.joint_names.append('gripper_joint_0')
		goal.trajectory.points.append(endpoint)

		# execute the motion
		try: 
			self._client.send_goal(self._action_topic, goal)
		except Exception as e:
			Logger.logwarn('Was unable to execute joint trajectory:\n%s' % str(e))
			self._failed = True


	def on_exit(self, userdata):
		if not self._client.has_result(self._action_topic):
			self._client.cancel(self._action_topic)
			Logger.loginfo("Cancelled active action goal.")

# 需要导入模块: from flexbe_core.proxy import ProxyActionClient [as 别名]
# 或者: from flexbe_core.proxy.ProxyActionClient import get_result [as 别名]
class CreateStepGoalState(EventState):
	'''
	Creates a footstep goal from a desired pose.

	-- pose_is_pelvis 	boolean 		Set this to True if the pose is given
										as pelvis pose and not on the ground.

	># target_pose 		PoseStamped 	Pose to which the robot should walk.

	#> step_goal 		Feet 			Desired feet pose.

	<= done 							Successfully created a step goal.
	<= failed 							Failed to create a plan.

	'''

	def __init__(self, pose_is_pelvis = False):
		'''Constructor'''

		super(CreateStepGoalState, self).__init__(outcomes = ['done', 'failed'],
												  input_keys = ['target_pose'],
												  output_keys = ['step_goal'])

		self._action_topic = "/vigir/footstep_manager/generate_feet_pose"

		self._client = ProxyActionClient({self._action_topic: GenerateFeetPoseAction})

		self._pose_is_pelvis = pose_is_pelvis

		self._done   = False
		self._failed = False


	def execute(self, userdata):
		'''Execute this state'''
		
		if self._failed:
			return 'failed'
		if self._done:
			return 'done'

		if self._client.has_result(self._action_topic):
			result = self._client.get_result(self._action_topic)

			if result.status.error == ErrorStatus.NO_ERROR:
				userdata.step_goal = result.feet
				self._done = True
				return 'done'
			else:
				Logger.logwarn('Step goal creation failed:\n%s' % result.status.error_msg)
				self._failed = True
				return 'failed'


	def on_enter(self, userdata):
		'''Upon entering the state, request the feet pose.'''
		
		self._done   = False
		self._failed = False

		# Create request msg and action goal
		request = FeetPoseRequest()
		request.header = userdata.target_pose.header
		request.header.stamp = rospy.Time.now() # timestamp used to track goal
		request.pose   = userdata.target_pose.pose
		request.flags  = FeetPoseRequest.FLAG_PELVIS_FRAME if self._pose_is_pelvis else 0

		action_goal = GenerateFeetPoseGoal(request = request)

		# Send action goal
		try:
			self._client.send_goal(self._action_topic, action_goal)
		except Exception as e:
			Logger.logwarn('Failed to send step goal request')
			rospy.logwarn(str(e))
			self._failed = True


	def on_exit(self, userdata):
		'''Destructor'''
		
		if not self._client.has_result(self._action_topic):
			self._client.cancel(self._action_topic)
			Logger.loginfo("Cancelled active action goal.")

# 需要导入模块: from flexbe_core.proxy import ProxyActionClient [as 别名]
# 或者: from flexbe_core.proxy.ProxyActionClient import get_result [as 别名]
class ExecuteStepPlanActionState(EventState):
	'''
	Implements a state to execute a step plan of the footstep planner via the OBFSM.
	This state will change the control mode of the robot to STEP during execution and expects to be in STAND when entered.

	># plan_header 	Header 		Header of the footstep plan to be executed.
								Use one of the footstep planning states to calculate such a plan.

	<= finished 				Finished walking.
								Control mode will be changed back to STAND, but this can take some time.
								If you need to be in STAND for the next state, add a CheckCurrentControlModeState.
	
	<= failed 					Failed to completely execute the plan. Plan might have been executed partially.

	'''


	def __init__(self):
		'''
		Constructor
		'''
		super(ExecuteStepPlanActionState , self).__init__(outcomes=['finished','failed'],
														  input_keys=['plan_header'])

		self._action_topic = "/vigir/footstep_manager/execute_step_plan"
		self._client = ProxyActionClient({self._action_topic: ExecuteStepPlanAction})

		self._failed = False


	def execute(self, userdata):
		'''
		Execute this state
		'''
		
		if self._failed:
			return 'failed'

		if self._client.has_result(self._action_topic):
			result = self._client.get_result(self._action_topic)
			
			if (result is not None and 
				result.status.status == FootstepExecutionStatus.REACHED_GOAL):
				return 'finished'
			else:
				if result is None:
					Logger.logwarn("Got None as result")
				else:
					Logger.logwarn("Result status %d" % result.status.status)
				self._failed = True
				return 'failed'


	def on_enter(self, userdata):
		'''Upon entering the state, request step plan execution from OBFSM'''
		
		execution_request = StepPlan(header = userdata.plan_header)

		action_goal = ExecuteStepPlanGoal(step_plan = execution_request)
		
		try:
			self._client.send_goal(self._action_topic, action_goal)
		except Exception as e:
			Logger.logwarn('Unable to execute step plan')
			rospy.logwarn(str(e))
			self._failed = True


	def on_exit(self, userdata):
		if not self._client.has_result(self._action_topic):
			self._client.cancel(self._action_topic)
			Logger.loginfo("Cancelled active action goal.")

# 需要导入模块: from flexbe_core.proxy import ProxyActionClient [as 别名]
# 或者: from flexbe_core.proxy.ProxyActionClient import get_result [as 别名]
class ExecuteTrajectoryState(EventState):
	'''
	Executes a custom trajectory.

	-- controller       string      One of the class constants to specify the controller which should execute the given trajectory.
	-- joint_names      string[]    List of the joint names. Order has to match the order of the provided values.

	># joint_positions  float[][]	Trajectory to be executed, given as list of time steps where each step contains a list of target joint values.
	># time 			float[]		Relative time in seconds from starting the execution when the corresponding time step should be reached.

	<= done 						Trajectory has successfully finished its execution.
	<= failed 						Failed to execute 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, controller, joint_names):
		'''
		Constructor
		'''
		super(ExecuteTrajectoryState, self).__init__(outcomes=['done', 'failed'],
														input_keys=['joint_positions', 'time'])

		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._joint_names = joint_names
		self._action_topic = "/" + controller_namespace + "/" + controller + "/follow_joint_trajectory"

		self._client = ProxyActionClient({self._action_topic: FollowJointTrajectoryAction})

		self._failed = False


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

		if self._client.has_result(self._action_topic):
			result = self._client.get_result(self._action_topic)
			if result.error_code == FollowJointTrajectoryResult.SUCCESSFUL:
				return 'done'
			else:
				Logger.logwarn('Joint trajectory request failed to execute: (%d) %s' % (result.error_code, result.error_string))
				return 'failed'


	def on_enter(self, userdata):
		# create goal
		goal = FollowJointTrajectoryGoal()
		goal.trajectory = JointTrajectory()
		goal.trajectory.joint_names = self._joint_names
		goal.trajectory.points = []

		for i in range(len(userdata.joint_positions)):
			point = JointTrajectoryPoint()
			point.positions = userdata.joint_positions[i]
			point.time_from_start = rospy.Duration.from_sec(userdata.time[i])
			goal.trajectory.points.append(point)

		# execute the motion
		try: 
			self._client.send_goal(self._action_topic, goal)
		except Exception as e:
			Logger.logwarn('Was unable to execute joint trajectory:\n%s' % str(e))
			self._failed = True

# 需要导入模块: from flexbe_core.proxy import ProxyActionClient [as 别名]
# 或者: from flexbe_core.proxy.ProxyActionClient import get_result [as 别名]
class ApproachPersonState(EventState):
	'''
	Actionlib actions are the most common basis for state implementations
	since they provide a non-blocking, high-level interface for robot capabilities.
	The example is based on the DoDishes-example of actionlib (see http://wiki.ros.org/actionlib).
	This time we have input and output keys in order to specify the goal and possibly further evaluate the result in a later state.

	-- dishes_to_do int 	Expected amount of dishes to be cleaned.

	># dishwasher 	int 	ID of the dishwasher to be used.

	#> cleaned 		int 	Amount of cleaned dishes.

	<= cleaned_some 		Only a few dishes have been cleaned.
	<= cleaned_enough		Cleaned a lot of dishes.
	<= command_error		Cannot send the action goal.

	'''

	def __init__(self):
		# See example_state.py for basic explanations.
		super(ApproachPersonState, self).__init__(outcomes = ['goal_reached', 'goal_failed','command_error'], input_keys = ['person_pose'])

		# Create the action client when building the behavior.
		# This will cause the behavior to wait for the client before starting execution
		# and will trigger a timeout error if it is not available.
		# Using the proxy client provides asynchronous access to the result and status
		# and makes sure only one client is used, no matter how often this state is used in a behavior.
		self._topic = 'stalk_pose'
		self._client = ProxyActionClient({self._topic: StalkPoseAction}) # pass required clients as dict (topic: type)
		self._timeoutTime = None

		self._personTopic = '/upper_body_detector/closest_bounding_box_centre'

		# It may happen that the action client fails to send the action goal.
		self._error = False

	def execute(self, userdata):
		# While this state is active, check if the action has been finished and evaluate the result.
		print 'approach_person_state: returning goal_reached'
		return 'goal_reached'
		# Check if the client failed to send the goal.
		if self._error:
			return 'command_error'

		if rospy.Time.now() > self._timeoutTime:
			return 'goal_failed'

		# Check if the action has been finished
		if self._client.has_result(self._topic):
			result = self._client.get_result(self._topic)

			if result.success:
				return 'goal_reached'

			else:
				return 'goal_failed'

		# If the action has not yet finished, no outcome will be returned and the state stays active.
		

	def on_enter(self, userdata):
		# When entering this state, we send the action goal once to let the robot start its work.

		# As documented above, we get the specification of which dishwasher to use as input key.
		# This enables a previous state to make this decision during runtime and provide the ID as its own output key.

		# Create the goal.
		goal = StalkPoseGoal() 

		goal.runtime_sec = 60
		goal.topic_name = self._personTopic
		goal.target = userdata.person_pose
		self._error = False

		self._timeoutTime = rospy.Time.now() + rospy.Duration(20)

		#try:
		#	self._client.send_goal(self._topic, goal)
		#except Exception as e:
				# Since a state failure not necessarily causes a behavior failure, it is recommended to only print warnings, not errors.
				# Using a linebreak before appending the error log enables the operator to collapse details in the GUI.
		#	Logger.logwarn('Failed to send the ApproachPerson command:\n%s' % str(e))
		#	self._error = True

	def on_exit(self, userdata):
		# Make sure that the action is not running when leaving this state.
		# A situation where the action would still be active is for example when the operator manually triggers an outcome.

		if not self._client.has_result(self._topic):
			self._client.cancel(self._topic)
			Logger.loginfo('Cancelled active action goal.')

# 需要导入模块: from flexbe_core.proxy import ProxyActionClient [as 别名]
# 或者: from flexbe_core.proxy.ProxyActionClient import get_result [as 别名]
class ChangeControlModeActionState(EventState):
	'''
	Implements a state where the robot changes its control mode using the action.

	-- target_mode 	string	The desired control mode to change to (e.g. "stand", "manipulate", etc.).
							The state's class variables can also be used (e.g. ChangeControlModeActionState.STAND).

	<= changed				Indicates successful transition to the desired control mode.
	<= failed				Indicates failure to either send the control mode change request
							or a failure to change the control mode on the Action server's side.

	'''

	NONE = "none" 
	FREEZE = "freeze" 
	STAND_PREP = "stand_prep" 
	STAND = "stand" 
	WALK = "walk" 
	STEP = "step" 
	MANIPULATE = "manipulate" 
	IMPEDANCE_STIFF = "manipulate_stiff_impedance"
	IMPEDANCE_COMPLIANT = "manipulate_compliant_impedance"
	IMPEDANCE_OBSERVER = "manipulate_observer_impedance"
	USER = "user" 
	CALIBRATE = "calibrate" 
	SOFT_STOP = "soft_stop" 
	STAND_MANIPULATE = "stand_manipulate" 
	WALK_MANIPULATE = "walk_manipulate" 
	STEP_MANIPULATE = "step_manipulate" 

	def __init__(self, target_mode):
		'''
		Constructor
		'''
		super(ChangeControlModeActionState, self).__init__(outcomes=['changed','failed'])

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

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

		self._action_topic = "/" + controller_namespace + "/control_mode_controller/change_control_mode"

		self._target_mode = target_mode

		self._client = ProxyActionClient({self._action_topic: ChangeControlModeAction})

		self._failed = False


	def execute(self, userdata):
		'''
		Execute this state
		'''
		if self._failed:
			return 'failed'

		if self._client.has_result(self._action_topic):
			result = self._client.get_result(self._action_topic)
			# result.status == 1 means SUCCESS
			if result is None or result.result.status != 1 or self._target_mode != result.result.current_control_mode:
				rospy.logwarn('Was unable to change the control mode to %s' % str(self._target_mode))
				self._failed = True
				return 'failed'
			else:
				return 'changed'


	def on_enter(self, userdata):

		self._failed = False

		action_goal = ChangeControlModeGoal(mode_request = self._target_mode)

		try:
			self._client.send_goal(self._action_topic, action_goal)
		except Exception as e:
			Logger.logwarn('Failed to send change control mode request')
			rospy.logwarn(str(e))
			self._failed = True

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

# 需要导入模块: from flexbe_core.proxy import ProxyActionClient [as 别名]
# 或者: from flexbe_core.proxy.ProxyActionClient import get_result [as 别名]
class MoveToFixedWaypoint(EventState):
	'''
	Lets the robot move to a given waypoint.

	-- allow_backwards 	Boolean 	Allow the robot to drive backwards when approaching the given waypoint.

	># waypoint		PoseStamped		Specifies the waypoint to which the robot should move.
	># speed					Speed of the robot

	<= reached 					Robot is now located at the specified waypoint.
	<= failed 					Failed to send a motion request to the action server.
	'''

	def __init__(self, allow_backwards = False):
		'''
		Constructor
		'''
		super(MoveToFixedWaypoint, self).__init__(outcomes=['reached', 'failed'],
											input_keys=['waypoint','speed'])
		
		self._action_topic = '/move_base'
		self._move_client = ProxyActionClient({self._action_topic: MoveBaseAction})
		#self.set_tolerance = rospy.ServiceProxy('/controller/set_alternative_tolerances', SetAlternativeTolerance)
		
		self._dynrec = Client("/vehicle_controller", timeout = 10)
		self._defaultspeed = 0.1

		self._allow_backwards = allow_backwards

		self._failed = False
		self._reached = False
		
		
	def execute(self, userdata):
		'''
		Execute this state
		'''
		
		if self._failed:
			return 'failed'
		if self._reached:
			return 'reached'

		if self._move_client.has_result(self._action_topic):
			result = self._move_client.get_result(self._action_topic)
			self._dynrec.update_configuration({'speed':self._defaultspeed})	
			if result.result == 1:
				self._reached = True
				return 'reached'
			else:
				self._failed = True
				Logger.logwarn('Failed to reach waypoint!')
				return 'failed'


			
	def on_enter(self, userdata):

		
		speedValue = self._dynrec.get_configuration(timeout = 0.5)
		if speedValue is not None:
			self._defaultspeed = speedValue['speed']	
			
		self._dynrec.update_configuration({'speed':userdata.speed})		

		self._startTime = Time.now()

		self._failed = False
		self._reached = False
		
		#goal_id = GoalID()
		#goal_id.id = 'abcd'
		#goal_id.stamp = Time.now()

		action_goal = MoveBaseGoal()
		action_goal.target_pose = userdata.waypoint
		action_goal.speed = userdata.speed
		action_goal.reverse_allowed = self._allow_backwards

		if action_goal.target_pose.header.frame_id == "":
			action_goal.target_pose.header.frame_id = "world"

		try:
			self._move_client.send_goal(self._action_topic, action_goal)
			#resp = self.set_tolerance(goal_id, 0.2, 1.55)
		except Exception as e:
			Logger.logwarn('Failed to send motion request to waypoint (%(x).3f, %(y).3f):\n%(err)s' % {
				'err': str(e),
				'x': userdata.waypoint.pose.position.x,
				'y': userdata.waypoint.pose.position.y
			})
			self._failed = True
		
		Logger.loginfo('Driving to next waypoint')
			

	def on_stop(self):
		try:
			if self._move_client.is_available(self._action_topic) \
			and not self._move_client.has_result(self._action_topic):
#.........这里部分代码省略.........

# 需要导入模块: from flexbe_core.proxy import ProxyActionClient [as 别名]
# 或者: from flexbe_core.proxy.ProxyActionClient import get_result [as 别名]
class PlanFootstepsState(EventState):
    """
	Creates a footstep plan to reach the desired feet pose via the OBFSM.

	-- mode 			string 		One of the available planning modes (class constants).

	># step_goal 		Feet 		Desired feet pose.

	#> plan_header 		Header 		The header of the footstep plan.

	<= planned 						Successfully created a plan.
	<= failed 						Failed to create a plan.

	"""

    MODE_STEP_NO_COLLISION = "drc_step_no_collision"
    MODE_STEP_2D = "drc_step_2D"
    MODE_STEP_3D = "drc_step_3D"
    MODE_WALK = "drc_walk"

    def __init__(self, mode):
        """
		Constructor
		"""
        super(PlanFootstepsState, self).__init__(
            outcomes=["planned", "failed"], input_keys=["step_goal"], output_keys=["plan_header"]
        )

        self._action_topic = "/vigir/footstep_manager/step_plan_request"

        self._client = ProxyActionClient({self._action_topic: StepPlanRequestAction})

        self._mode = mode

        self._done = False
        self._failed = False

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

        if self._failed:
            userdata.plan_header = None
            return "failed"
        if self._done:
            return "planned"

        if self._client.has_result(self._action_topic):
            result = self._client.get_result(self._action_topic)
            if result.status.warning != ErrorStatus.NO_WARNING:
                Logger.logwarn("Planning footsteps warning:\n%s" % result.status.warning_msg)

            if result.status.error == ErrorStatus.NO_ERROR:
                userdata.plan_header = result.step_plan.header

                num_steps = len(result.step_plan.steps)
                Logger.loginfo("Received plan with %d steps" % num_steps)

                self._done = True
                return "planned"
            else:
                userdata.plan_header = None
                Logger.logerr("Planning footsteps failed:\n%s" % result.status.error_msg)
                self._failed = True
                return "failed"

    def on_enter(self, userdata):
        """Upon entering the state, send the footstep plan request."""

        self._failed = False
        self._done = False

        # Create request msg and action goal
        request = StepPlanRequest()
        request.header = userdata.step_goal.header
        request.max_planning_time = 10.0
        request.parameter_set_name = String(self._mode)

        action_goal = StepPlanRequestGoal(plan_request=request)

        try:
            self._client.send_goal(self._action_topic, action_goal)
        except Exception as e:
            Logger.logwarn("Failed to send footstep plan request:\n%s" % str(e))
            self._failed = True

    def on_exit(self, userdata):
        """Destructor"""

        if not self._client.has_result(self._action_topic):
            self._client.cancel(self._action_topic)
            Logger.loginfo("Cancelled active action goal.")

# 需要导入模块: from flexbe_core.proxy import ProxyActionClient [as 别名]
# 或者: from flexbe_core.proxy.ProxyActionClient import get_result [as 别名]
class MoveitStartingPointState(EventState):
	"""
	Uses moveit to plan and move to the first point of a given arm trajectory.

	-- vel_scaling 		float 		Scales the velocity of the planned trajectory,
									lower values for slower trajectories.

	># trajectories		dict 		arm side (str) : joint values (float[])

	"""
	
	def __init__(self, vel_scaling = 0.1):
		"""Constructor"""
		super(MoveitStartingPointState, self).__init__(outcomes=['reached', 'failed'],
													   input_keys=['trajectories'])

		self._action_topic = "/vigir_move_group"

		self._client = ProxyActionClient({self._action_topic: MoveAction})

		self._vel_scaling = vel_scaling

		self._failed = False


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

		if self._client.has_result(self._action_topic):
			result = self._client.get_result(self._action_topic)
			
			if result.error_code.val is MoveItErrorCodes.SUCCESS:
				return 'reached'
			else:
				Logger.logwarn('Motion plan request failed with the following error code: %d' % result.error_code.val)
				self._failed = True
				return 'failed'
	
	
	def on_enter(self, userdata):
		"""Upon entering the state, get the first trajectory point and request moveit motion plan to get there."""

		self._failed = False

		# Create the motion goal (one for both arms)
		move_group_goal = MoveGoal()
		move_group_goal.request = MotionPlanRequest()

		# Figure out which planning group is needed
		if len(userdata.trajectories) is 2:
			self._planning_group = "both_arms_group"
		else:
			chain = userdata.trajectories.keys()[0]
			if chain is 'left_arm':
				self._planning_group = "l_arm_group"
			elif chain is 'right_arm':
				self._planning_group = "r_arm_group"
			if chain is 'left_leg':
				self._planning_group = "l_leg_group"
			elif chain is 'right_leg':
				self._planning_group = "r_leg_group"
			else:
				Logger.logwarn('Unexpected key: %s Either "left" or "right" was expected.' % arm)
		
		move_group_goal.request.group_name = self._planning_group
		move_group_goal.request.max_velocity_scaling_factor = self._vel_scaling

		# Get first/starting trajectory point
		move_group_goal.request.goal_constraints = [Constraints()]
		move_group_goal.request.goal_constraints[0].joint_constraints = []

		for arm, traj in userdata.trajectories.iteritems():

			joint_names = traj.joint_names

			starting_point = traj.points[0].positions

			if len(joint_names) != len(starting_point):
				Logger.logwarn("Number of joint names (%d) does not match number of joint values (%d)" % (len(joint_names), len(starting_point)))

			for i in range(min(len(joint_names), len(starting_point))):
				move_group_goal.request.goal_constraints[0].joint_constraints.append(JointConstraint(joint_name = joint_names[i], 
																									 position = starting_point[i])
				)

		# Send the motion plan request to the server
		try: 
			Logger.loginfo("Moving %s to starting point." % self._planning_group)
			self._client.send_goal(self._action_topic, move_group_goal)
		except Exception as e:
			Logger.logwarn('Was unable to execute %s motion request:\n%s' % (self._planning_group, str(e)))
			self._failed = True

# 需要导入模块: from flexbe_core.proxy import ProxyActionClient [as 别名]
# 或者: from flexbe_core.proxy.ProxyActionClient import get_result [as 别名]
class GotoSingleArmJointConfigState(EventState):
    """
	Directly commands the trajectory/joint controllers to move a 
	single joint to the desired configuration.

	-- target_config 	int 		Identifier of the pre-defined pose to be used.
	-- arm_side			string		Arm side {left, right}
 
	># current_config 	dict		The current arm joint positions
									joint_names string[] : joint_values[]

	<= done 						Successfully executed the motion.
	<= failed 						Failed to execute the motion.

	"""

    # Wrists
    WRIST_CCW = 11
    WRIST_CW = 12

    # Forearms
    # ...

    def __init__(self, arm_side, target_config, time=2.0):
        """Constructor"""
        super(GotoSingleArmJointConfigState, self).__init__(outcomes=["done", "failed"], input_keys=["current_config"])

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

        self._robot = rospy.get_param("behavior/robot_namespace")

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

        ################################ ATLAS ################################
        self._configs = dict()
        self._configs["flor"] = dict()
        self._configs["flor"]["left"] = {
            11: {"joint_name": "l_arm_wry2", "joint_value": -2.5},
            12: {"joint_name": "l_arm_wry2", "joint_value": +2.5},
        }
        self._configs["flor"]["right"] = {
            11: {"joint_name": "r_arm_wry2", "joint_value": +2.5},
            12: {"joint_name": "r_arm_wry2", "joint_value": -2.5},
        }
        ################################ THOR #################################
        self._configs["thor_mang"] = dict()
        self._configs["thor_mang"]["left"] = {
            11: {"joint_name": "l_wrist_yaw2", "joint_value": 3.84},
            12: {"joint_name": "l_wrist_yaw2", "joint_value": -3.84},
        }
        self._configs["thor_mang"]["right"] = {
            11: {"joint_name": "r_wrist_yaw2", "joint_value": -3.84},
            12: {"joint_name": "r_wrist_yaw2", "joint_value": 3.84},
        }
        #######################################################################

        self._joint_name = self._configs[self._robot][arm_side][target_config]["joint_name"]
        self._joint_value = self._configs[self._robot][arm_side][target_config]["joint_value"]
        self._time = time

        self._action_topic = (
            "/" + controller_namespace + "/" + arm_side + "_arm_traj_controller" + "/follow_joint_trajectory"
        )

        self._client = ProxyActionClient({self._action_topic: FollowJointTrajectoryAction})

        self._failed = False

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

        if self._failed:
            return "failed"

        if self._client.has_result(self._action_topic):
            result = self._client.get_result(self._action_topic)
            if result:
                if result.error_code == FollowJointTrajectoryResult.SUCCESSFUL:
                    return "done"
                else:
                    Logger.logwarn(
                        "Joint trajectory failed to execute (%d). Reason: %s" % (result.error_code, result.error_string)
                    )
                    self._failed = True
                    return "failed"
            else:
                Logger.logwarn("Wait for result returned True even though the result is %s" % str(result))
                self._failed = True
                return "failed"

    def on_enter(self, userdata):
        """On enter, create and send the follow joint trajectory action goal."""

        self._failed = False
#.........这里部分代码省略.........

# 需要导入模块: from flexbe_core.proxy import ProxyActionClient [as 别名]
# 或者: from flexbe_core.proxy.ProxyActionClient import get_result [as 别名]
class FootstepPlanTurnState(EventState):
	'''
	Implements a state where the robot plans a motion to turn in place, e.g. 90 degree to the left.
	Please note that the angle is only approximate, actual distance depends on exact step alignment.

	-- direction 		int 	One of the class constants to specify a turning direction.

	># angle 			float 	Angle to turn, given in degrees.

	#> plan_header 		Header  Plan to perform the turning motion.

	<= planned                 	Successfully created a plan.
	<= failed                  	Failed to create a plan.

	'''
	TURN_LEFT = 5      # PatternParameters.ROTATE_LEFT
	TURN_RIGHT = 6     # PatternParameters.ROTATE_RIGHT

	def __init__(self, direction):
		'''
		Constructor
		'''
		super(FootstepPlanTurnState, self).__init__(outcomes=['planned', 'failed'],
													input_keys=['angle'],
													output_keys=['plan_header'])

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

		self._degrees_per_step = rospy.get_param("behavior/turn_degrees_per_step")
		
		self._action_topic = '/vigir/footstep_manager/step_plan_request'

		self._client = ProxyActionClient({self._action_topic: StepPlanRequestAction})

		self._failed = False
		self._direction = direction
		
		
	def execute(self, userdata):
		if self._failed:
			userdata.plan_header = None
			return 'failed'

		if self._client.has_result(self._action_topic):
			result = self._client.get_result(self._action_topic)
			if result.status.warning != ErrorStatus.NO_WARNING:
				Logger.logwarn('Planning footsteps warning:\n%s' % result.status.warning_msg)

			if result.status.error == ErrorStatus.NO_ERROR:
				userdata.plan_header = result.step_plan.header
				return 'planned'
			else:
				userdata.plan_header = None # as recommended: dont send out incomplete plan
				Logger.logerr('Planning footsteps failed:\n%s' % result.status.error_msg)
				return 'failed'
		

	def on_enter(self, userdata):
		# Create footstep planner request
		pattern_parameters = PatternParameters()
		pattern_parameters.mode = self._direction
		pattern_parameters.turn_angle = math.radians(self._degrees_per_step)
		pattern_parameters.close_step = True
		pattern_parameters.steps = int(round(userdata.angle / self._degrees_per_step))

		request = StepPlanRequest()
		request.parameter_set_name = String('drc_step_no_collision')
		request.header = Header(frame_id = '/world', stamp = rospy.Time.now())
		request.planning_mode = StepPlanRequest.PLANNING_MODE_PATTERN
		request.pattern_parameters = pattern_parameters

		action_goal = StepPlanRequestGoal(plan_request = request)

		try:
			self._client.send_goal(self._action_topic, action_goal)
		except Exception as e:
			Logger.logwarn('Was unable to create footstep pattern for wide stance:\n%s' % str(e))
			self._failed = True

# 需要导入模块: from flexbe_core.proxy import ProxyActionClient [as 别名]
# 或者: from flexbe_core.proxy.ProxyActionClient import get_result [as 别名]
class MoveArmDynState(EventState):
	'''
	Lets the robot move its arm.

	># object_pose		PoseStamped		Pose of the object to observe.
	># object_type		string			Object type.
	># object_id		string			ID of the object.

	<= reached 						Target joint configuration has been reached.
	<= sampling_failed				Failed to find any valid and promising camera pose.
	<= planning_failed 				Failed to find a plan to move the arm to a desired camera pose.
	<= control_failed 				Failed to move the arm along the planned trajectory.

	'''


	def __init__(self):
		'''
		Constructor
		'''
		super(MoveArmDynState, self).__init__(outcomes=['reached', 'sampling_failed', 'planning_failed', 'control_failed'],
											input_keys=['object_pose', 'object_type', 'object_id'])

		self._action_topic = '/combined_planner'
		self._client = ProxyActionClient({self._action_topic: ArgoCombinedPlanAction})
		
		# rotations for different object types to get z-axis aligned with optical axis
		self._rot = tf.transformations.quaternion_about_axis(-0.5*math.pi, (0,1,0))

		self._sampling_failed = False
		self._planning_failed = False
		self._control_failed = False
		self._success = False


	def execute(self, userdata):
		'''
		Execute this state
		'''
		if self._sampling_failed:
			return 'sampling_failed'
		if self._planning_failed:
			return 'planning_failed'
		if self._control_failed:
			return 'control_failed'
		if self._success:
			return 'reached'

		if self._client.has_result(self._action_topic):
			result = self._client.get_result(self._action_topic)

			if result.success.val == ErrorCodes.SUCCESS:
				self._success = True
				return 'reached'
			elif result.success.val == ErrorCodes.SAMPLING_FAILED:
				Logger.logwarn('Sampling failed when attempting to move the arm')
				self._sampling_failed = True
				return 'sampling_failed'	
			elif result.success.val == ErrorCodes.PLANNING_FAILED:
				Logger.logwarn('Planning failed when attempting to move the arm')
				self._planning_failed = True
				return 'planning_failed'
			else:
				Logger.logwarn('Control failed when attempting to move the arm')
				self._control_failed = True
				return 'control_failed'



	def on_enter(self, userdata):
		self._sampling_failed = False
		self._planning_failed = False
		self._control_failed = False
		self._success = False

		action_goal = ArgoCombinedPlanGoal()
		action_goal.target = deepcopy(userdata.object_pose)
		
		type_map = {
			'dial_gauge': ObjectTypes.DIAL_GAUGE,
			'level_gauge': ObjectTypes.LEVEL_GAUGE,
			'valve': ObjectTypes.VALVE,
			'hotspot': ObjectTypes.HOTSPOT,
			'pipes' : ObjectTypes.PIPES,
			'victim' : ObjectTypes.VICTIM
		}
		object_type = type_map.get(userdata.object_type, ObjectTypes.UNKNOWN)
		
		q = [ action_goal.target.pose.orientation.x, action_goal.target.pose.orientation.y,
			  action_goal.target.pose.orientation.z, action_goal.target.pose.orientation.w ]
		q = tf.transformations.quaternion_multiply(q, self._rot)
		action_goal.target.pose.orientation = Quaternion(*q)
		
		action_goal.object_id.data = userdata.object_id
		action_goal.object_type.val = object_type
		action_goal.action_type.val = ActionCodes.SAMPLE_MOVE_ARM

		Logger.loginfo('Position arm to look at %s object' % str(userdata.object_type))
		
		try:
#.........这里部分代码省略.........

# 需要导入模块: from flexbe_core.proxy import ProxyActionClient [as 别名]
# 或者: from flexbe_core.proxy.ProxyActionClient import get_result [as 别名]
class MoveToWaypointState(EventState):
	'''
	Lets the robot move to a given waypoint.

	># waypoint		PoseStamped		Specifies the waypoint to which the robot should move.

	<= reached 						Robot is now located at the specified waypoint.
	<= failed 						Failed to send a motion request to the action server.

	'''

	def __init__(self):
		'''
		Constructor
		'''
		super(MoveToWaypointState, self).__init__(outcomes=['reached', 'failed'],
											input_keys=['waypoint'])
		
		self._action_topic = '/move_base'
		self._client = ProxyActionClient({self._action_topic: MoveBaseAction})

		self._failed = False
		self._reached = False
		
		
	def execute(self, userdata):
		'''
		Execute this state
		'''
		if self._failed:
			return 'failed'
		if self._reached:
			return 'reached'

		if self._client.has_result(self._action_topic):
			result = self._client.get_result(self._action_topic)
			if result.result == 1:
				self._reached = True
				return 'reached'
			else:
				self._failed = True
				Logger.logwarn('Failed to reach waypoint!')
				return 'failed'

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

		action_goal = MoveBaseGoal()
		action_goal.target_pose = userdata.waypoint
		if action_goal.target_pose.header.frame_id == "":
			action_goal.target_pose.header.frame_id = "world"

		try:
			self._client.send_goal(self._action_topic, action_goal)
		except Exception as e:
			Logger.logwarn('Failed to send motion request to waypoint (%(x).3f, %(y).3f):\n%(err)s' % {
				'err': str(e),
				'x': userdata.waypoint.pose.position.x,
				'y': userdata.waypoint.pose.position.y
			})
			self._failed = True
		
		Logger.loginfo('Driving to next waypoint')
			

	def on_stop(self):
		try:
			if self._client.is_available(self._action_topic) \
			and not self._client.has_result(self._action_topic):
				self._client.cancel(self._action_topic)
		except:
			# client already closed
			pass

	def on_pause(self):
		self._client.cancel(self._action_topic)

	def on_resume(self, userdata):
		self.on_enter(userdata)