Python ProxyActionClient.send_goal方法代码示例

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

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

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

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

	'''

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

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

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

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

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

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

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

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

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

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

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

		if self._failed:
			return 'failed'

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


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

		self._failed = False

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

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

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


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

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