Python ProxyActionClient.is_available方法代码示例

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

	># waypoint		PoseStamped		Specifies the waypoint to which the robot should move.
	># victim		string			object_id of detected object
	># 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.
	<= update					Update the pose of current waypoint
	'''

	def __init__(self):
		'''
		Constructor
		'''
		super(MoveToWaypointState, self).__init__(outcomes=['reached', 'failed', 'update'],
											input_keys=['waypoint','victim','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._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)
			if result.result == 1:
				self._reached = True
				return 'reached'
			else:
				self._failed = True
				Logger.logwarn('Failed to reach waypoint!')
				return 'failed'

		self._currentTime = Time.now()
		if self._currentTime.secs - self._startTime.secs >= 10:
			return 'update'

			
	def on_enter(self, userdata):

		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

		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):
				self._move_client.cancel(self._action_topic)
		except:
			# client already closed
			pass

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

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

#.........这里部分代码省略.........
                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._param_error     = False
                self._planning_failed = False
                self._control_failed  = False
                self._success         = False

                #Parameter check
                if self._srdf_param is None:
                        self._param_error = True
                        return

                try:
                        self._srdf = ET.fromstring(self._srdf_param)
                except Exception as e:
                        Logger.logwarn('Unable to parse given SRDF parameter: /robot_description_semantic')
                        self._param_error = True

                if not self._param_error:

                        robot = None
                        for r in self._srdf.iter('robot'):
                                if self._robot_name == '' or self._robot_name == r.attrib['name']:
                                        robot = r
                                        break
                        if robot is None:
                                Logger.logwarn('Did not find robot name in SRDF: %s' % self._robot_name)
                                return 'param_error'

                        config = None
                        for c in robot.iter('group_state'):
                                if (self._move_group == '' or self._move_group == c.attrib['group']) \
                                and c.attrib['name'] == self._config_name:
                                        config = c
                                        self._move_group = c.attrib['group'] #Set move group name in case it was not defined
                                        break
                        if config is None:
                                Logger.logwarn('Did not find config name in SRDF: %s' % self._config_name)
                                return 'param_error'

                        try:
                                self._joint_config = [float(j.attrib['value']) for j in config.iter('joint')]
                                self._joint_names  = [str(j.attrib['name']) for j in config.iter('joint')]
                        except Exception as e:
                                Logger.logwarn('Unable to parse joint values from SRDF:\n%s' % str(e))
                                return 'param_error'


                        #Action Initialization
                        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=self._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 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):
                self.on_enter(userdata)

# 需要导入模块: from flexbe_core.proxy import ProxyActionClient [as 别名]
# 或者: from flexbe_core.proxy.ProxyActionClient import is_available [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 is_available [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 is_available [as 别名]
class LookAtPattern(EventState):
	'''
	Specify a pattern for the robots camera to follow

	># pattern		string				The pattern to follow

	<= succeeded 						Camera follows the pattern
	<= failed 						Failed to set pattern

	'''

	def __init__(self):
		'''
		Constructor
		'''
		super(LookAtPattern, self).__init__(outcomes=['succeeded', 'failed'],
											input_keys=['pattern'])
		
		self._action_topic = '/pan_tilt_sensor_head_joint_control/look_at'
		self._client = ProxyActionClient({self._action_topic: hector_perception_msgs.msg.LookAtAction})

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


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

		action_goal = hector_perception_msgs.msg.LookAtGoal()
		action_goal.look_at_target.pattern = userdata.pattern

		try:
			self._client.send_goal(self._action_topic, action_goal)
		except Exception as e:
			Logger.logwarn('Failed to send LookAt request with pattern %(pat)s\n%(err)s' % {
				'pat': userdata.pattern,
				'err': str(e)
				
			})
			self._failed = True
		
		Logger.loginfo('Pattern %(pat)s loaded' % { 'pat': userdata.pattern })
		self._succeeded = 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 is_available [as 别名]
class LookAtWaypoint(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(LookAtWaypoint, self).__init__(outcomes=["reached", "failed"], input_keys=["waypoint"])

        self._action_topic = "/pan_tilt_sensor_head_joint_control/look_at"
        self._client = ProxyActionClient({self._action_topic: hector_perception_msgs.msg.LookAtAction})

        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)
            # if result.result == 1:
            # self._reached = True
            # return 'reached'
            # else:
            # self._failed = True
            # Logger.logwarn('Failed to look at waypoint!')
            # return 'failed'

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

        action_goal = hector_perception_msgs.msg.LookAtGoal()
        action_goal.look_at_target.target_point.point = userdata.waypoint.pose.position
        if action_goal.look_at_target.target_point.header.frame_id == "":
            action_goal.look_at_target.target_point.header.frame_id = "map"

        try:
            self._client.send_goal(self._action_topic, action_goal)
        except Exception as e:
            Logger.logwarn(
                "Failed to send LookAt 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("Looking at next waypoint")
        self._reached = 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 is_available [as 别名]
class PipeDetectionState(EventState):
    """
	Triggers pipe detection to find the location of the pipes

	-- max_attempts 	int 			Maximum number of re-running attempts.
										0 means unlimitied.

	<= found 							Found the pipe location.
	<= unknown 							Did not detect any pipes.

	"""

    def __init__(self, max_attempts=1):
        """
		Constructor
		"""
        super(PipeDetectionState, self).__init__(outcomes=["found", "unknown"])

        self._action_topic = "/hector_five_pipes_detection_node/detect"
        self._client = ProxyActionClient({self._action_topic: DetectObjectAction})

        self._max_attempts = max_attempts
        self._success = False
        self._failed = False

    def execute(self, userdata):
        """
		Execute this state
		"""
        if self._failed:
            return "unknown"

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

            if result.detection_success:
                return "found"
            elif self._max_attempts == 0:
                Logger.loginfo("Did not detect pipes, trying again (unlimited)...")
                self.on_enter(userdata)
            elif self._max_attempts > 1:
                self._max_attempts -= 1
                Logger.loginfo("Did not detect pipes, trying again (%d more times)..." % self._max_attempts)
                self.on_enter(userdata)
            else:
                return "unknown"

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

        action_goal = DetectObjectGoal()

        try:
            self._client.send_goal(self._action_topic, action_goal)
        except Exception as e:
            Logger.logwarn("Failed to send pipe detection request:\n%s" % str(e))
            self._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 is_available [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 is_available [as 别名]

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