Python ProxyActionClient.has_result方法代码示例

# 需要导入模块: from flexbe_core.proxy import ProxyActionClient [as 别名]
# 或者: from flexbe_core.proxy.ProxyActionClient import has_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 has_result [as 别名]
class MoveCameraState(EventState):
	'''
	Moves the camera.

	># pan 		float 	Pan angle [-180, 180)
	># tilt 	float 	Tilt angle [-41, 41]

	<= done			Moved the camera.
	<= failed		Cannot send the action goal.

	'''

	def __init__(self):
		# See example_state.py for basic explanations.
		super(MoveCameraState, self).__init__(outcomes = ['done', 'failed'],
												 input_keys = ['pan', 'tilt'])

		self._topic = 'SetPTUState'
		self._client = ProxyActionClient({self._topic: PtuGotoAction})

		self._error = False


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

		if self._client.has_result(self._topic):
			return 'done'


	def on_enter(self, userdata):
		goal = PtuGotoGoal()
		goal.pan = userdata.pan
		goal.tilt = userdata.tilt
		goal.pan_vel = 60
		goal.tilt_vel = 60

		self._error = False
		try:
			self._client.send_goal(self._topic, goal)
		except Exception as e:
			Logger.logwarn('Failed to send the camera movement 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 has_result [as 别名]
class SpeechOutputState(EventState):
	'''
	Lets the robot speak the given input string.

	># text 	string 	Text to output.

	<= done 			Speaking has finished.
	<= failed 			Failed to execute speaking.

	'''

	def __init__(self):
		super(SpeechOutputState, self).__init__(outcomes = ['done', 'failed'],
												input_keys = ['text'])

		self._topic = '/speak'
		self._client = ProxyActionClient({self._topic: maryttsAction})

		self._error = False


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

		if self._client.has_result(self._topic):
			return 'done'

		
	def on_enter(self, userdata):
		goal = maryttsGoal()
		goal.text = userdata.text

		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 has_result [as 别名]
class SpeechOutputState(EventState):
    """
	Lets the robot speak the given input string.

	># text 	string 	Text to output.

	<= done 			Speaking has finished.
	<= failed 			Failed to execute speaking.

	"""

    def __init__(self):
        super(SpeechOutputState, self).__init__(outcomes=["done", "failed"], input_keys=["text"])

        self._topic = "/speak"
        self._client = ProxyActionClient({self._topic: maryttsAction})

        self._error = False

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

        if self._client.has_result(self._topic):
            return "done"

    def on_enter(self, userdata):
        goal = maryttsGoal()
        goal.text = userdata.text
        Logger.loginfo("Speech output, saying:\n%s" % goal.text)

        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 has_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 has_result [as 别名]
class StartExploration(EventState):
	'''
	Starts the Exploration Task via /move_base

	
	<= succeeded			Exploration Task was successful
	<= failed 			Exploration Task failed

	'''

	def __init__(self):
		super(StartExploration, self).__init__(outcomes = ['succeeded', 'failed'])
		
		self._action_topic = '/move_base'
		self._move_client = ProxyActionClient({self._action_topic: MoveBaseAction})

		self._succeeded = False
		self._failed = False

	def execute(self, userdata):
		if self._move_client.has_result(self._action_topic):
			result = self._move_client.get_result(self._action_topic)
			if result.result == 1:
				self._reached = True
				Logger.loginfo('Exploration succeeded')
				return 'succeeded'
			else:
				self._failed = True
				Logger.logwarn('Exploration failed!')
				return 'failed'
		

	def on_enter(self, userdata):

		self._succeeded = False
		self._failed = False
		
		action_goal = MoveBaseGoal()
		action_goal.exploration = True
		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)
		except Exception as e:
			Logger.logwarn('Failed to start Exploration' % {
				'err': str(e),
				'x': userdata.waypoint.pose.position.x,
				'y': userdata.waypoint.pose.position.y
			})
			self._failed = True
		
		


	def on_exit(self, userdata):
		pass


	def on_start(self):
		pass

	def on_stop(self):
		pass

# 需要导入模块: from flexbe_core.proxy import ProxyActionClient [as 别名]
# 或者: from flexbe_core.proxy.ProxyActionClient import has_result [as 别名]
class MoveitToJointsDynState(EventState):
	'''
	Uses MoveIt to plan and move the specified joints to the target configuration.

	-- move_group		string		Name of the move group to be used for planning.
									Specified joint names need to exist in the given group.
	-- action_topic 	string 		Topic on which MoveIt is listening for action calls.

	># joint_names		string[]	Names of the joints to set.
									Does not need to specify all joints.
	># joint_values		float[]		Target configuration of the joints.
									Same order as their corresponding names in joint_names.

	<= reached 						Target joint configuration has been reached.
	<= planning_failed 				Failed to find a plan to the given joint configuration.
	<= control_failed 				Failed to move the arm along the planned trajectory.

	'''


	def __init__(self, move_group, action_topic = '/move_group'):
		'''
		Constructor
		'''
		super(MoveitToJointsDynState, self).__init__(
			outcomes=['reached', 'planning_failed', 'control_failed'],
			input_keys=['joint_values', 'joint_names'])
		
		self._action_topic = action_topic
		self._client = ProxyActionClient({self._action_topic: MoveGroupAction})

		self._move_group = move_group
		self._joint_names = None

		self._planning_failed = False
		self._control_failed = False
		self._success = False
		
		
	def execute(self, userdata):
		'''
		Execute this state
		'''
		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.error_code.val == MoveItErrorCodes.CONTROL_FAILED:
				Logger.logwarn('Control failed for move action of group: %s (error code: %s)' % (self._move_group, str(result.error_code)))
				self._control_failed = True
				return 'control_failed'
			elif result.error_code.val != MoveItErrorCodes.SUCCESS:
				Logger.logwarn('Move action failed with result error code: %s' % str(result.error_code))
				self._planning_failed = True
				return 'planning_failed'
			else:
				self._success = True
				return 'reached'

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

		self._joint_names = userdata.joint_names

		action_goal = MoveGroupGoal()
		action_goal.request.group_name = self._move_group
		goal_constraints = Constraints()
		for i in range(len(self._joint_names)):
			goal_constraints.joint_constraints.append(JointConstraint(joint_name=self._joint_names[i], position=userdata.joint_values[i]))
		action_goal.request.goal_constraints.append(goal_constraints)

		try:
			self._client.send_goal(self._action_topic, action_goal)
		except Exception as e:
			Logger.logwarn('Failed to send action goal for group: %s\n%s' % (self._move_group, str(e)))
			self._planning_failed = True
			

	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):
#.........这里部分代码省略.........

# 需要导入模块: from flexbe_core.proxy import ProxyActionClient [as 别名]
# 或者: from flexbe_core.proxy.ProxyActionClient import has_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 has_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 has_result [as 别名]
class GripperRollState(EventState):
    """
	Open or close the gripper.

	-- rotation 	float 	Rotation of the joint, use HORIZONTAL for horizontal pose (0).
	-- duration 	float 	Time (sec) for executing the motion.

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

	"""

    HORIZONTAL = 0

    def __init__(self, rotation, duration=1.0):
        """
		Constructor
		"""
        super(GripperRollState, self).__init__(outcomes=["done", "failed"])

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

        self._failed = False
        self._duration = duration
        self._target_joint_value = rotation

    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("arm_joint_4")
        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 has_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 has_result [as 别名]
class InputState(EventState):
	'''
	Implements a state where the state machine needs an input from the operator.
	Requests of different types, such as requesting a waypoint, a template, or a pose, can be specified.

	-- request 	uint8 		One of the class constants to specify the type of request.
	-- message 	string 		Message displayed to the operator to let him know what to do.

	#> data 	object 		The data provided by the operator. The exact type depends on the request.

	<= received 			Returned as soon as valid data is available.
	<= aborted 				The operator declined to provide the requested data.
	<= no_connection 		No request could be sent to the operator.
	<= data_error 			Data has been received, but could not be deserialized successfully.

	'''

	POINT_LOCATION			= BehaviorInputGoal.POINT_LOCATION
	SELECTED_OBJECT_ID		= BehaviorInputGoal.SELECTED_OBJECT_ID
	WAYPOINT_GOAL_POSE		= BehaviorInputGoal.WAYPOINT_GOAL_POSE
	GHOST_JOINT_STATES		= BehaviorInputGoal.GHOST_JOINT_STATES
	FOOTSTEP_PLAN_HEADER	= BehaviorInputGoal.FOOTSTEP_PLAN_HEADER


	def __init__(self, request, message):
		'''
		Constructor
		'''
		super(InputState, self).__init__(outcomes=['received', 'aborted', 'no_connection', 'data_error'],
												output_keys=['data'])
		
		self._action_topic = '/flexbe/behavior_input'

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

		self._request = request
		self._message = message
		self._connected = True
		self._received = False
		
		
	def execute(self, userdata):
		'''
		Execute this state
		'''
		if not self._connected:
			return 'no_connection'
		if self._received:
			return 'received'

		if self._client.has_result(self._action_topic):
			result = self._client.get_result(self._action_topic)
			if result.result_code != BehaviorInputResult.RESULT_OK:
				userdata.data = None
				return 'aborted'
			else:
				try:
					response_data = pickle.loads(result.data)
					#print 'Input request response:', response_data
					userdata.data = response_data
					# If this was a template ID request, log that template ID:
					if self._request == BehaviorInputGoal.SELECTED_OBJECT_ID:
						Logger.loginfo('Received template ID: %d' % int(response_data))
				
				except Exception as e:
					Logger.logwarn('Was unable to load provided data:\n%s' % str(e))
					userdata.data = None
					return 'data_error'

				self._received = True
				return 'received'
			
	
	def on_enter(self, userdata):

		self._received = False

		if not self._connected: return

		action_goal = BehaviorInputGoal()
		action_goal.request_type = self._request
		action_goal.msg = self._message

		try:
			self._client.send_goal(self._action_topic, action_goal)
		except Exception as e:
			Logger.logwarn('Was unable to send data request:\n%s' % str(e))
			self._connected = False

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

	># joint_config		float[]		Target joint configuration of the arm.

	<= reached 						Target joint configuration has been reached.
	<= planning_failed 						Failed to find a plan to the given joint configuration.
	<= control_failed 						Failed to move the arm along the planned trajectory.

	'''


	def __init__(self):
		'''
		Constructor
		'''
		super(MoveArmState, self).__init__(outcomes=['reached', 'planning_failed', 'control_failed'],
											input_keys=['joint_config', 'group_name'])
		
		self._action_topic = '/move_group'
		self._client = ProxyActionClient({self._action_topic: MoveGroupAction})

		self._joint_names = ['arm_joint_0', 'arm_joint_1', 'arm_joint_2', 'arm_joint_3']

		self._planning_failed = False
		self._control_failed = False
		self._success = False
		
		
	def execute(self, userdata):
		'''
		Execute this state
		'''
		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.error_code.val == MoveItErrorCodes.CONTROL_FAILED:
				Logger.logwarn('Move Action failed with result error code: %s' % str(result.error_code))
				self._control_failed = True
				return 'control_failed'
			elif result.error_code.val != MoveItErrorCodes.SUCCESS:
				Logger.logwarn('Move Action failed with result error code: %s' % str(result.error_code))
				self._planning_failed = True
				return 'planning_failed'
			else:
				self._success = True
				return 'reached'

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

		action_goal = MoveGroupGoal()
		action_goal.request.group_name = userdata.group_name
		goal_constraints = Constraints()
		for i in range(len(self._joint_names)):
			goal_constraints.joint_constraints.append(JointConstraint(joint_name=self._joint_names[i], position=userdata.joint_config[i]))
		action_goal.request.goal_constraints.append(goal_constraints)

		try:
			self._client.send_goal(self._action_topic, action_goal)
		except Exception as e:
			Logger.logwarn('Failed to send move group goal for arm motion:\n%s' % str(e))
			self._planning_failed = True
			

	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)

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

    -- gripper_cmd      int     Identifier of the pre-defined commands.
                                The state's class variables can be used here.
    -- time             float   Relative time in seconds from starting
                                the execution to when the corresponding
                                target gripper state should be reached.

    <= done                     Gripper command was successfully executed.
    <= failed                   Failed to send or execute gripper command.
    """

    CLOSE_GRIPPER = 0
    OPEN_GRIPPER = 1

    def __init__(self, gripper_cmd, time=5.0):
        """Constructor"""

        super(GripperCommandState, self).__init__(outcomes=["done", "failed"])

        self._joint_names = ["gripper_finger_joint_l", "gripper_finger_joint_r"]

        self._gripper_joint_positions = {
            #  gripper_finger_joint_l, gripper_finger_joint_r
            0: [0.001, 0.001],
            1: [0.010, 0.010],  # NOTE: 0.011 results in goal tolerance violation
        }

        self._gripper_cmd = gripper_cmd
        self._time = time

        self._action_topic = "/arm_1/gripper_controller/follow_joint_trajectory"

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

        self._done = False
        self._failed = False

    def execute(self, userdata):
        """Wait for action result and return outcome accordingly"""

        if self._done:
            return "done"
        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:
                self._done = True
                return "done"
            elif result.error_code == FollowJointTrajectoryResult.GOAL_TOLERANCE_VIOLATED:
                Logger.logwarn("Probably done, but goal tolerances violated (%d)" % result.error_code)
                self._done = True
                return "done"
            else:
                Logger.logwarn(
                    "Gripper trajectory failed to execute: (%d) %s" % (result.error_code, result.error_string)
                )
                self._failed = True
                return "failed"

    def on_enter(self, userdata):
        """Create and send action goal"""

        self._done = False
        self._failed = False

        # Create and populate action goal
        goal = FollowJointTrajectoryGoal()
        goal.trajectory.joint_names = self._joint_names

        target_joint_positions = self._gripper_joint_positions[self._gripper_cmd]

        point = JointTrajectoryPoint()
        point.positions = target_joint_positions
        point.time_from_start = rospy.Duration.from_sec(self._time)
        goal.trajectory.points.append(point)

        # Send the action goal for execution
        try:
            self._client.send_goal(self._action_topic, goal)
        except Exception as e:
            Logger.logwarn("Unable to send follow joint trajectory action goal:\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 has_result [as 别名]
class MotionServiceState(EventState):
    """Implements a state where a certain motion is performed.
    This state can be used to execute motions created by the motion editor.

    -- motion_key 	string 	Reference to the motion to be executed.
    -- time_factor	float	Factor to multiply with the default execution time of the specified motion.
                            For example, 2 will result in a motion twice as long, thus executed at half speed.

    <= done 				Indicates that the expected execution time of the motion has passed.
                            The motion service provides no way to check if a motion has actually finished.
    <= failed               Indicates that the requested motion doesn't exist and therefore wasn't executed
    """

    def __init__(self, motion_key, time_factor=1):
        """
        Constructor
        """
        super(MotionServiceState, self).__init__(outcomes=['done', 'failed'])

        self.motion_key = motion_key
        self.time_factor = time_factor
        self._finish_time = None
        self._motion_goal_ns = '/motion_service/motion_goal'

        self._client = ProxyActionClient({self._motion_goal_ns: ExecuteMotionAction})

        self._failed = False
        self._done = False

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

        if self._client.has_result(self._motion_goal_ns):
            result = self._client.get_result(self._motion_goal_ns)
            if result is None:  # Bug in actionlib, sometimes returns None instead of result
                # Operator decision needed
                Logger.logwarn("Failed to execute the motion '%s':\nAction result is None" % self.motion_key)
                self._failed = True
                return 'failed'
            if result.error_string is None or len(result.error_code) == 0:
                Logger.logwarn("Failed to execute the motion '%s':\nAction result is None" % self.motion_key)
                self._failed = True
                return 'failed'

            success = all(lambda x: x == 0, result.error_code)

            if success:
                rospy.loginfo('Trajectory finished successfully.') # dont need to send this to the operator
                self._done = True
                return 'done'
            else:
                Logger.logwarn("Failed to execute the motion '%s':\n%s" % (self.motion_key, str(result.error_code)))
                self._failed = True
                return 'failed'


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

        # build goal
        goal = ExecuteMotionGoal()
        goal.motion_name = self.motion_key
        goal.time_factor = self.time_factor

        try:
            self._client.send_goal(self._motion_goal_ns, goal)
        except Exception as e:
            Logger.logwarn("Failed sending motion goal for '%s':\n%s" % (self.motion_key, str(e)))

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