Python ProxyActionClient.cancel方法代码示例

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

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

	-- 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):
		if not self._client.has_result(self._active_topic):
			self._client.cancel(self._active_topic)
			Logger.loginfo("Cancelled active action goal.")

# 需要导入模块: from flexbe_core.proxy import ProxyActionClient [as 别名]
# 或者: from flexbe_core.proxy.ProxyActionClient import cancel [as 别名]
class TweetPictureState(EventState):
    '''
    State for tweeting a picture and text

    '''

    def __init__(self):
        # See example_state.py for basic explanations.
        super(TweetPictureState, self).__init__(outcomes = ['picture_tweeted', 'tweeting_failed','command_error'], input_keys = ['picture_path', 'tweet_text'])

        # 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 = 'strands_tweets'
        self._client = ProxyActionClient({self._topic: SendTweetAction}) # pass required clients as dict (topic: type)

        # 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.

        # Check if the client failed to send the goal.
        if self._error:
            return 'command_error'

        # 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 'picture_tweeted'

            else:
                return 'tweeting_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.
        tweet = SendTweetGoal() 
        tweet.text = userdata.tweet_text
        
        if len(userdata.tweet_text) > 140:
            tweet.text = '#LAMoR15 #ECMR15 I just told a too looong joke, stupid tweet length!'

        tweet.with_photo = True
        img = cv2.imread(userdata.picture_path)

        bridge = CvBridge()
        image_message = bridge.cv2_to_imgmsg(img, encoding="bgr8")
        tweet.photo = image_message

        try:
            self._client.send_goal(self._topic, tweet)
        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 TweetPictureState 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 cancel [as 别名]

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

        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

        current_config = userdata.current_config

        # Get the index of the joint whose value will be replaced
        index_of_joint = current_config["joint_names"].index(self._joint_name)

        # Replace the old joint value with the target_config's one
        new_values = current_config["joint_values"]
        new_values[index_of_joint] = self._joint_value

        # Create trajectory point out of the revised joint values
        point = JointTrajectoryPoint(positions=new_values, time_from_start=rospy.Duration.from_sec(self._time))

        # Create trajectory message
        trajectory = JointTrajectory(joint_names=current_config["joint_names"], points=[point])

        action_goal = FollowJointTrajectoryGoal(trajectory=trajectory)

        # execute the motion
        try:
            self._client.send_goal(self._action_topic, action_goal)
        except Exception as e:
            Logger.logwarn("Failed to send trajectory action goal:\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 cancel [as 别名]
class MoveBaseState(EventState):
    """
    Navigates a robot to a desired position and orientation using move_base.

    ># waypoint     Pose2D      Target waypoint for navigation.

    <= arrived                  Navigation to target pose succeeded.
    <= failed                   Navigation to target pose failed.
    """

    def __init__(self):
        """Constructor"""

        super(MoveBaseState, self).__init__(outcomes = ['arrived', 'failed'],
                                            input_keys = ['waypoint'])

        self._action_topic = "/move_base"

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

        self._arrived = False
        self._failed = False


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

        if self._arrived:
            return 'arrived'
        if self._failed:
            return 'failed'

        if self._client.has_result(self._action_topic):
            status = self._client.get_state(self._action_topic)
            if status == GoalStatus.SUCCEEDED:
                self._arrived = True
                return 'arrived'
            elif status in [GoalStatus.PREEMPTED, GoalStatus.REJECTED,
                            GoalStatus.RECALLED, GoalStatus.ABORTED]:
                Logger.logwarn('Navigation failed: %s' % str(status))
                self._failed = True
                return 'failed'


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

        self._arrived = False
        self._failed = False

        # Create and populate action goal
        goal = MoveBaseGoal()

        pt = Point(x = userdata.waypoint.x, y = userdata.waypoint.y)
        qt = transformations.quaternion_from_euler(0, 0, userdata.waypoint.theta)

        goal.target_pose.pose = Pose(position = pt,
                                     orientation = Quaternion(*qt))

        goal.target_pose.header.frame_id = "odom"
        # goal.target_pose.header.stamp.secs = 5.0

        # Send the action goal for execution
        try:
            self._client.send_goal(self._action_topic, goal)
        except Exception as e:
            Logger.logwarn("Unable to send navigation 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 cancel [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 cancel [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)

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

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

	'''

	UP_40 	 = -40.0 # max -40 degrees
	UP_20 	 = -20.0
	STRAIGHT = +0.00
	DOWN_30  = +20.0
	DOWN_45  = +40.0
	DOWN_60	 = +60.0 # max +60 degrees

	# HEAD
	# STRAIGHT = 0
	# UP_40    = 1 
	# UP_20    = 2
	# DOWN_30  = 3
	# DOWN_45  = 4
	# DOWN_60  = 5 

	def __init__(self, desired_tilt):
		'''Constructor'''

		super(TiltHeadState, self).__init__(outcomes=['done', 'failed'])


		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._configs['flor']['same'] =  {
		# 	20: {'joint_name': 'neck_ry', 'joint_values': [+0.00], 'controller': 'neck_traj_controller'}, # max -40 degrees
		# 	21: {'joint_name': 'neck_ry', 'joint_values': [-40.0], 'controller': 'neck_traj_controller'},
		# 	22: {'joint_name': 'neck_ry', 'joint_values': [-20.0], 'controller': 'neck_traj_controller'},
		# 	23: {'joint_name': 'neck_ry', 'joint_values': [+20.0], 'controller': 'neck_traj_controller'},
		# 	24: {'joint_name': 'neck_ry', 'joint_values': [+40.0], 'controller': 'neck_traj_controller'},
		# 	25: {'joint_name': 'neck_ry', 'joint_values': [+60.0], 'controller': 'neck_traj_controller'}  # max +60 degrees
		# }

		self._action_topic = "/" + controller_namespace + \
							 "/neck_traj_controller" + "/follow_joint_trajectory"

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

		# Convert desired position to radians
		self._desired_tilt = math.radians(desired_tilt)

		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:
				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))
					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):
		'''Upon entering, create a neck trajectory and send the action goal.'''

		self._failed = False

		# Create neck point
		neck_point = JointTrajectoryPoint()
		neck_point.time_from_start = rospy.Duration.from_sec(1.0)
		neck_point.positions.append(self._desired_tilt)

		# Create neck trajectory
		neck_trajectory = JointTrajectory()
		neck_trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2)
		neck_trajectory.joint_names = ['neck_ry']
		neck_trajectory.points.append(neck_point)

		# Create action goal
		action_goal = FollowJointTrajectoryGoal(trajectory = neck_trajectory)

		# execute the motion
		try: 
			self._client.send_goal(self._action_topic, action_goal)
		except Exception as e:
			Logger.logwarn('Failed to send neck trajectory 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 cancel [as 别名]
class Explore(EventState):
	'''
	Starts the Exploration Task via /move_base

	># speed			Speed of the robot

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

	'''

	def __init__(self):
		super(Explore, self).__init__(outcomes = ['succeeded', 'failed'], input_keys =['speed'])
		
		self._action_topic = '/move_base'
		self._move_client = ProxyActionClient({self._action_topic: MoveBaseAction})
		self._dynrec = Client("/vehicle_controller", timeout = 10)
		self._defaultspeed = 0.1
		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)
			self._dynrec.update_configuration({'speed':self._defaultspeed})	
			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):
			
		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._succeeded = False
		self._failed = False
		
		action_goal = MoveBaseGoal()
		action_goal.exploration = True
		action_goal.speed = userdata.speed

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

		try:
			if self._move_client.is_active(self._action_topic):
				self._move_client.cancel(self._action_topic)
			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):
		self._move_client.cancel(self._action_topic)


	def on_start(self):
		pass

	def on_stop(self):
		pass