Python SimpleActionServer.set_preempted方法代码示例

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import set_preempted [as 别名]
class BaseRotate():
  def __init__(self):
    self._action_name = 'base_rotate'
    
    #initialize base controller
    topic_name = '/base_controller/command'
    self._base_publisher = rospy.Publisher(topic_name, Twist)

    #initialize this client
    self._as = SimpleActionServer(self._action_name, BaseRotateAction, execute_cb=self.run, auto_start=False)
    self._as.start()
    rospy.loginfo('%s: started' % self._action_name)
    
  def run(self, goal):
    rospy.loginfo('Rotating base')
    count = 0
    r = rospy.Rate(10)
    while count < 70:
      if self._as.is_preempt_requested():
        self._as.set_preempted()
        return

      twist_msg = Twist()
      twist_msg.linear = Vector3(0.0, 0.0, 0.0)
      twist_msg.angular = Vector3(0.0, 0.0, 1.0)

      self._base_publisher.publish(twist_msg)
      count = count + 1
      r.sleep()

    self._as.set_succeeded()    

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import set_preempted [as 别名]
class PipolFollower():
    def __init__(self):
        
        rospy.loginfo("Creating Pipol follower AS: '" + PIPOL_FOLLOWER_AS + "'")
        self._as = SimpleActionServer(PIPOL_FOLLOWER_AS, PipolFollowAction, 
                                      execute_cb = self.execute_cb,
                                      preempt_callback = self.preempt_cb, 
                                      auto_start = False)
        rospy.loginfo("Starting " + PIPOL_FOLLOWER_AS)
        self._as.start()
        
    def execute_cb(self, goal):
        print "goal is: " + str(goal)
        # helper variables
        success = True
        
        # start executing the action
        for i in xrange(1, goal.order):
          # check that preempt has not been requested by the client
          if self._as.is_preempt_requested():
            rospy.loginfo('%s: Preempted' % self._action_name)
            self._as.set_preempted()
            success = False
            break
          self._feedback.sequence.append(self._feedback.sequence[i] + self._feedback.sequence[i-1])
          # publish the feedback
          self._as.publish_feedback(self._feedback)
          # this step is not necessary, the sequence is computed at 1 Hz for demonstration purposes
          r.sleep()
          
        if success:
          self._result.sequence = self._feedback.sequence
          rospy.loginfo('%s: Succeeded' % self._action_name)
          self._as.set_succeeded(self._result)  

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import set_preempted [as 别名]
class TrajectoryExecution:

    def __init__(self, side_prefix):
        
        traj_controller_name = '/' + side_prefix + '_arm_controller/joint_trajectory_action'
        self.traj_action_client = SimpleActionClient(traj_controller_name, JointTrajectoryAction)
        rospy.loginfo('Waiting for a response from the trajectory action server arm: ' + side_prefix)
        self.traj_action_client.wait_for_server()
        rospy.loginfo('Trajectory executor is ready for arm: ' + side_prefix)
    
        motion_request_name = '/' + side_prefix + '_arm_motion_request/joint_trajectory_action'
        self.action_server = SimpleActionServer(motion_request_name, JointTrajectoryAction, execute_cb=self.move_to_joints)
        self.action_server.start()
	self.has_goal = False

    def move_to_joints(self, traj_goal):
        rospy.loginfo('Receieved a trajectory execution request.')
    	traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1))
        self.traj_action_client.send_goal(traj_goal)
	rospy.sleep(1)
	is_terminated = False
	while(not is_terminated):
	    action_state = self.traj_action_client.get_state()
	    if (action_state == GoalStatus.SUCCEEDED):
		self.action_server.set_succeeded()
		is_terminated = True
	    elif (action_state == GoalStatus.ABORTED):
		self.action_server.set_aborted()
		is_terminated = True
	    elif (action_state == GoalStatus.PREEMPTED):
		self.action_server.set_preempted()
		is_terminated = True
	rospy.loginfo('Trajectory completed.')

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import set_preempted [as 别名]
class ReadyArmActionServer:
    def __init__(self):
        self.move_arm_client = SimpleActionClient('/move_left_arm', MoveArmAction)
        self.ready_arm_server = SimpleActionServer(ACTION_NAME,
                                                   ReadyArmAction,
                                                   execute_cb=self.ready_arm,
                                                   auto_start=False)
                                                   
    def initialize(self):
        rospy.loginfo('%s: waiting for move_left_arm action server', ACTION_NAME)
        self.move_arm_client.wait_for_server()
        rospy.loginfo('%s: connected to move_left_arm action server', ACTION_NAME)
        
        self.ready_arm_server.start()
        
    def ready_arm(self, goal):
        if self.ready_arm_server.is_preempt_requested():
            rospy.loginfo('%s: preempted' % ACTION_NAME)
            self.move_arm_client.cancel_goal()
            self.ready_arm_server.set_preempted()
            
        if move_arm_joint_goal(self.move_arm_client,
                               ARM_JOINTS,
                               READY_POSITION,
                               collision_operations=goal.collision_operations):
            self.ready_arm_server.set_succeeded()
        else:
            rospy.logerr('%s: failed to ready arm, aborting', ACTION_NAME)
            self.ready_arm_server.set_aborted()

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import set_preempted [as 别名]

#.........这里部分代码省略.........
    def read_wrist_tilt(self, rotate_data):
        self.wrist_tilt = rotate_data
        self.has_latest_wrist_tilt = True


    def wait_for_latest_controller_states(self, timeout):
        self.has_latest_shoulder_pan = False
        self.has_latest_arm_tilt = False
        self.has_latest_elbow_tilt = False
        self.has_latest_wrist_pan = False
        self.has_latest_wrist_tilt = False
        r = rospy.Rate(100)
        start = rospy.Time.now()
        while (self.has_latest_shoulder_pan == False or self.has_latest_arm_tilt == False or \
                self.has_latest_elbow_tilt == False or self.has_latest_wrist_tilt == False or self.has_latest_wrist_pan == False) and \
                (rospy.Time.now() - start < timeout):
            r.sleep()


    def transform_target_point(self, point):
        rospy.loginfo("%s: Retrieving IK solutions", NAME)
        rospy.wait_for_service('smart_arm_ik_service', 10)
        ik_service = rospy.ServiceProxy('smart_arm_ik_service', SmartArmIK)
        resp = ik_service(point)
        if (resp and resp.success):
            return resp.solutions[0:4]
        else:
            raise Exception, "Unable to obtain IK solutions."


    def execute_callback(self, goal):
        r = rospy.Rate(100)
        self.result.success = True
        self.result.arm_position = [self.shoulder_pan.process_value, self.arm_tilt.process_value, \
                self.elbow_tilt.process_value, self.wrist_pan.process_value, self.wrist_tilt.process_value]
        rospy.loginfo("%s: Executing move arm", NAME)
        
        # Initialize target joints
        target_joints = list()
        for i in range(self.JOINTS_COUNT):
            target_joints.append(0.0)
        
        # Retrieve target joints from goal
        if (len(goal.target_joints) > 0):
            for i in range(min(len(goal.target_joints), len(target_joints))):
                target_joints[i] = goal.target_joints[i] 
        else:
            try:
                # Convert target point to target joints (find an IK solution)
                target_joints = self.transform_target_point(goal.target_point)
            except (Exception, tf.Exception, tf.ConnectivityException, tf.LookupException):
                rospy.loginfo("%s: Aborted: IK Transform Failure", NAME)
                self.result.success = False
                self.server.set_aborted()
                return

        # Publish goal to controllers
        self.shoulder_pan_pub.publish(target_joints[0])
        self.arm_tilt_pub.publish(target_joints[1])
        self.elbow_tilt_pub.publish(target_joints[2])
        self.wrist_pan_pub.publish(target_joints[3])
        self.wrist_tilt_pub.publish(target_joints[4])
        
        # Initialize loop variables
        start_time = rospy.Time.now()

        while (math.fabs(target_joints[0] - self.shoulder_pan.process_value) > self.ERROR_THRESHOLD or \
                math.fabs(target_joints[1] - self.arm_tilt.process_value) > self.ERROR_THRESHOLD or \
                math.fabs(target_joints[2] - self.elbow_tilt.process_value) > self.ERROR_THRESHOLD or \
                math.fabs(target_joints[3] - self.wrist_pan.process_value) > self.ERROR_THRESHOLD or \
                math.fabs(target_joints[4] - self.wrist_tilt.process_value) > self.ERROR_THRESHOLD):
		
	        # Cancel exe if another goal was received (i.e. preempt requested)
            if self.server.is_preempt_requested():
                rospy.loginfo("%s: Aborted: Action Preempted", NAME)
                self.result.success = False
                self.server.set_preempted()
                break

            # Publish current arm position as feedback
            self.feedback.arm_position = [self.shoulder_pan.process_value, self.arm_tilt.process_value, \
                    self.elbow_tilt.process_value, self.wrist_pan.process_value, self.wrist_tilt.process_value]
            self.server.publish_feedback(self.feedback)
            
            # Abort if timeout
            current_time = rospy.Time.now()
            if (current_time - start_time > self.TIMEOUT_THRESHOLD):
                rospy.loginfo("%s: Aborted: Action Timeout", NAME)
                self.result.success = False
                self.server.set_aborted()
                break

            r.sleep()

        if (self.result.success):
            rospy.loginfo("%s: Goal Completed", NAME)
            self.wait_for_latest_controller_states(rospy.Duration(2.0))
            self.result.arm_position = [self.shoulder_pan.process_value, self.arm_tilt.process_value, \
                    self.elbow_tilt.process_value, self.wrist_pan.process_value, self.wrist_tilt.process_value]
            self.server.set_succeeded(self.result)

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import set_preempted [as 别名]

#.........这里部分代码省略.........
    def reset_head_position(self):
        self.head_pan_pub.publish(0.0)
        self.head_tilt_pub.publish(0.0)
        rospy.sleep(5)


    def wait_for_latest_controller_states(self, timeout):
        self.has_latest_pan = False
        self.has_latest_tilt = False
        r = rospy.Rate(100)
        start = rospy.Time.now()
        while (self.has_latest_pan == False or self.has_latest_tilt == False) and (rospy.Time.now() - start < timeout):
            r.sleep()


    def transform_target_point(self, point):
        pan_target_frame = self.head_pan_frame
        tilt_target_frame = self.head_tilt_frame
        
        # Wait for tf info (time-out in 5 seconds)
        self.tf.waitForTransform(pan_target_frame, point.header.frame_id, rospy.Time(), rospy.Duration(5.0))
        self.tf.waitForTransform(tilt_target_frame, point.header.frame_id, rospy.Time(), rospy.Duration(5.0))

        # Transform target point to pan reference frame & retrieve the pan angle
        pan_target = self.tf.transformPoint(pan_target_frame, point)
        pan_angle = math.atan2(pan_target.point.y, pan_target.point.x)
        #rospy.loginfo("%s: Pan transformed to <%s, %s, %s> => angle %s", \
        #        NAME, pan_target.point.x, pan_target.point.y, pan_target.point.z, pan_angle)

        # Transform target point to tilt reference frame & retrieve the tilt angle
        tilt_target = self.tf.transformPoint(tilt_target_frame, point)
        tilt_angle = math.atan2(tilt_target.point.z,
                math.sqrt(math.pow(tilt_target.point.x, 2) + math.pow(tilt_target.point.y, 2)))
        #rospy.loginfo("%s: Tilt transformed to <%s, %s, %s> => angle %s", \
        #        NAME, tilt_target.point.x, tilt_target.point.y, tilt_target.point.z, tilt_angle)

        return [pan_angle, tilt_angle]


    def execute_callback(self, goal):
        r = rospy.Rate(100)
        self.result.success = True
        self.result.head_position = [self.head_pan.process_value, self.head_tilt.process_value]
        rospy.loginfo("%s: Executing move head", NAME)
        
        # Initialize target joints
        target_joints = list()
        for i in range(self.JOINTS_COUNT):
            target_joints.append(0.0)
        
        # Retrieve target joints from goal
        if (len(goal.target_joints) > 0):
            for i in range(min(len(goal.target_joints), len(target_joints))):
                target_joints[i] = goal.target_joints[i] 
        else:
            try:
                # Try to convert target point to pan & tilt angles
                target_joints = self.transform_target_point(goal.target_point)
            except (tf.Exception, tf.ConnectivityException, tf.LookupException):
                rospy.loginfo("%s: Aborted: Transform Failure", NAME)
                self.result.success = False
                self.server.set_aborted()
                return

	    # Publish goal command to controllers
        self.head_pan_pub.publish(target_joints[0])
        self.head_tilt_pub.publish(target_joints[1])

        # Initialize loop variables
        start_time = rospy.Time.now()

        while (math.fabs(target_joints[0] - self.head_pan.process_value) > self.ERROR_THRESHOLD or \
                math.fabs(target_joints[1] - self.head_tilt.process_value) > self.ERROR_THRESHOLD):
		
	        # Cancel exe if another goal was received (i.e. preempt requested)
            if self.server.is_preempt_requested():
                rospy.loginfo("%s: Aborted: Action Preempted", NAME)
                self.result.success = False
                self.server.set_preempted()
                break

            # Publish current head position as feedback
            self.feedback.head_position = [self.head_pan.process_value, self.head_tilt.process_value]
            self.server.publish_feedback(self.feedback)
            
            # Abort if timeout
            current_time = rospy.Time.now()
            if (current_time - start_time > self.TIMEOUT_THRESHOLD):
                rospy.loginfo("%s: Aborted: Action Timeout", NAME)
                self.result.success = False
                self.server.set_aborted()
                break

            r.sleep()
        
        if (self.result.success):
            rospy.loginfo("%s: Goal Completed", NAME)
            self.wait_for_latest_controller_states(rospy.Duration(2.0))
            self.result.head_position = [self.head_pan.process_value, self.head_tilt.process_value]
            self.server.set_succeeded(self.result)

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import set_preempted [as 别名]

#.........这里部分代码省略.........
                rospy.loginfo("%s: Aborted: Action Preempted", NAME)
                self.gripper_client.cancel_goal()
                return GoalStatus.PREEMPTED

        return self.gripper_client.get_state()

#This method is used passes updates from arm positions actions to the feedback
#of this server
    def gripper_position_feedback_cb(self, fb):
        self.feedback.gripper_position = fb.gripper_position
        if self.server.is_active():
            self.server.publish_feedback(self.feedback)

#This method sets the results of the goal to the last feedback values
    def set_results(self, success):
        self.result.success = success
        self.result.arm_position = self.feedback.arm_position
        self.result.gripper_position = self.feedback.gripper_position
	self.server.set_succeeded(self.result)

#This is the callback function that is executed whenever the server 
#recieves a request
    def execute_callback(self, goal):
	
        rospy.loginfo("%s: Executing Grasp Action [%s, %f, %f, %f]", NAME, \
			goal.target_point.header.frame_id, goal.target_point.point.x, \
			goal.target_point.point.y, goal.target_point.point.z)

        rospy.loginfo( "%s: Moving the arm to the cobra pose", NAME)
    	#move the arm into the cobra position
    	result = self.move_arm(0.0, 1.972222, -1.972222, 0.0)
    	if result == GoalStatus.PREEMPTED:	#action has failed
    	    rospy.loginfo("%s: 1st Move Arm (to cobra pose) Preempted", NAME)
    	    self.server.set_preempted()
    	    self.set_results(False)
    	    return		
    	elif result != GoalStatus.SUCCEEDED:
    	    rospy.loginfo("%s: 1st Move Arm (to cobra pose) Failed", NAME)
    	    self.set_results(False)
    	    return

        position = Point(x = goal.target_point.point.x, y = goal.target_point.point.y)
        orientation = Quaternion(w=1.0)
        self.move_to('/map', position, orientation, 0.5)
        self.move_to('/map', position, orientation, 0.2)
        rospy.sleep(0.2)

        rospy.loginfo( "%s: Opening gripper", NAME)
        #open the gripper
        result = self.move_gripper(0.2, -0.2)
        if result == GoalStatus.PREEMPTED:	#action has failed
            rospy.loginfo("%s: Open Gripper Preempted", NAME)
            self.server.set_preempted()
            self.set_results(False)
            return		
        elif result != GoalStatus.SUCCEEDED:
            rospy.loginfo("%s: Open Gripper Failed", NAME)
            self.set_results(False)
            return
            
        rospy.loginfo( "%s: Moving the arm to the object", NAME)
	#move the arm to the correct posisions
        result = self.reach_at(goal.target_point.header.frame_id, \
                goal.target_point.point.x, \
                goal.target_point.point.y, \
                goal.target_point.point.z)

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import set_preempted [as 别名]
class PlaceObjectActionServer:
    def __init__(self):
        self.start_audio_recording_srv = rospy.ServiceProxy('/audio_dump/start_audio_recording', StartAudioRecording)
        self.stop_audio_recording_srv = rospy.ServiceProxy('/audio_dump/stop_audio_recording', StopAudioRecording)
        self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus)
        
        self.posture_controller = SimpleActionClient('/wubble_gripper_grasp_action', GraspHandPostureExecutionAction)
        self.move_arm_client = SimpleActionClient('/move_left_arm', MoveArmAction)
        
        self.attached_object_pub = rospy.Publisher('/attached_collision_object', AttachedCollisionObject)
        self.action_server = SimpleActionServer(ACTION_NAME,
                                                PlaceObjectAction,
                                                execute_cb=self.place_object,
                                                auto_start=False)

    def initialize(self):
        rospy.loginfo('%s: waiting for audio_dump/start_audio_recording service' % ACTION_NAME)
        rospy.wait_for_service('audio_dump/start_audio_recording')
        rospy.loginfo('%s: connected to audio_dump/start_audio_recording service' % ACTION_NAME)
        
        rospy.loginfo('%s: waiting for audio_dump/stop_audio_recording service' % ACTION_NAME)
        rospy.wait_for_service('audio_dump/stop_audio_recording')
        rospy.loginfo('%s: connected to audio_dump/stop_audio_recording service' % ACTION_NAME)
        
        rospy.loginfo('%s: waiting for wubble_gripper_grasp_action' % ACTION_NAME)
        self.posture_controller.wait_for_server()
        rospy.loginfo('%s: connected to wubble_gripper_grasp_action' % ACTION_NAME)
        
        rospy.loginfo('%s: waiting for move_left_arm action server' % ACTION_NAME)
        self.move_arm_client.wait_for_server()
        rospy.loginfo('%s: connected to move_left_arm action server' % ACTION_NAME)
        
        self.action_server.start()

    def open_gripper(self):
        pg = GraspHandPostureExecutionGoal()
        pg.goal = GraspHandPostureExecutionGoal.RELEASE
        
        self.posture_controller.send_goal(pg)
        self.posture_controller.wait_for_result()

    def place_object(self, goal):
        if self.action_server.is_preempt_requested():
            rospy.loginfo('%s: preempted' % ACTION_NAME)
            self.action_server.set_preempted()
            
        collision_object_name = goal.collision_object_name
        collision_support_surface_name = goal.collision_support_surface_name
        
        # check that we have something in hand before placing it
        grasp_status = self.get_grasp_status_srv()
        
        # if the object is still in hand after lift is done we are good
        if not grasp_status.is_hand_occupied:
            rospy.logerr('%s: gripper empty, nothing to place' % ACTION_NAME)
            self.action_server.set_aborted()
            return
            
        gripper_paddings = [LinkPadding(l,0.0) for l in GRIPPER_LINKS]
        
        # disable collisions between attached object and table
        collision_operation1 = CollisionOperation()
        collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
        collision_operation1.object2 = collision_support_surface_name
        collision_operation1.operation = CollisionOperation.DISABLE
        
        # disable collisions between gripper and table
        collision_operation2 = CollisionOperation()
        collision_operation2.object1 = collision_support_surface_name
        collision_operation2.object2 = GRIPPER_GROUP_NAME
        collision_operation2.operation = CollisionOperation.DISABLE
        collision_operation2.penetration_distance = 0.01
        
        # disable collisions between arm and table
        collision_operation3 = CollisionOperation()
        collision_operation3.object1 = collision_support_surface_name
        collision_operation3.object2 = ARM_GROUP_NAME
        collision_operation3.operation = CollisionOperation.DISABLE
        collision_operation3.penetration_distance = 0.01
        
        ordered_collision_operations = OrderedCollisionOperations()
        ordered_collision_operations.collision_operations = [collision_operation1, collision_operation2, collision_operation3]
        
        self.start_audio_recording_srv(InfomaxAction.PLACE, goal.category_id)
        
        if move_arm_joint_goal(self.move_arm_client,
                               ARM_JOINTS,
                               PLACE_POSITION,
                               link_padding=gripper_paddings,
                               collision_operations=ordered_collision_operations):
            sound_msg = None
            grasp_status = self.get_grasp_status_srv()
            
            self.open_gripper()
            rospy.sleep(0.5)
            
            # if after lowering arm gripper is still holding an object life's good
            if grasp_status.is_hand_occupied:
                sound_msg = self.stop_audio_recording_srv(True)
            else:
#.........这里部分代码省略.........

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import set_preempted [as 别名]

#.........这里部分代码省略.........
        pos_spacing=0.03,
        rot_spacing=0.1,
        consistent_angle=math.pi / 7.0,
        collision_aware=True,
        collision_check_resolution=1,
        steps_before_abort=1,
        num_steps=0,
        ordered_collision_operations=None,
        frame="base_link",
    ):

        start_pose = self.create_pose_stamped(approachpos + approachquat, frame)
        goal_pose = self.create_pose_stamped(grasppos + graspquat, frame)

        return self.get_interpolated_ik_motion_plan(
            start_pose,
            goal_pose,
            start_angles,
            ARM_JOINTS,
            pos_spacing,
            rot_spacing,
            consistent_angle,
            collision_aware,
            collision_check_resolution,
            steps_before_abort,
            num_steps,
            ordered_collision_operations,
            frame,
        )

    def push_object(self, goal):
        if self.action_server.is_preempt_requested():
            rospy.loginfo("%s: preempted" % ACTION_NAME)
            self.action_server.set_preempted()

        collision_object_name = goal.collision_object_name
        collision_support_surface_name = goal.collision_support_surface_name

        current_state = rospy.wait_for_message("l_arm_controller/state", FollowJointTrajectoryFeedback)
        start_angles = current_state.actual.positions

        full_state = rospy.wait_for_message("/joint_states", JointState)

        req = GetPositionFKRequest()
        req.header.frame_id = "base_link"
        req.fk_link_names = [GRIPPER_LINK_FRAME]
        req.robot_state.joint_state = full_state

        if not self.set_planning_scene_diff_client():
            rospy.logerr("%s: failed to set planning scene diff" % ACTION_NAME)
            self.action_server.set_aborted()
            return

        pose = self.get_fk_srv(req).pose_stamped[0].pose

        frame_id = "base_link"

        approachpos = [pose.position.x, pose.position.y, pose.position.z]
        approachquat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]

        push_distance = 0.40
        grasppos = [pose.position.x, pose.position.y - push_distance, pose.position.z]
        graspquat = [0.00000, 0.00000, 0.70711, -0.70711]

        # attach object to gripper, they will move together
        obj = AttachedCollisionObject()

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import set_preempted [as 别名]
class PathExecutor:

    goal_index = 0
    reached_all_nodes = True

    def __init__(self):
        rospy.loginfo('__init__ start')

        # create a node
        rospy.init_node(NODE)

        # Action server to receive goals
        self.path_server = SimpleActionServer('/path_executor/execute_path', ExecutePathAction,
                                              self.handle_path, auto_start=False)
        self.path_server.start()

        # publishers & clients
        self.visualization_publisher = rospy.Publisher('/path_executor/current_path', Path)

        # get parameters from launch file
        self.use_obstacle_avoidance = rospy.get_param('~use_obstacle_avoidance', True)
        # action server based on use_obstacle_avoidance
        if self.use_obstacle_avoidance == False:
            self.goal_client = SimpleActionClient('/motion_controller/move_to', MoveToAction)
        else:
            self.goal_client = SimpleActionClient('/bug2/move_to', MoveToAction)

        self.goal_client.wait_for_server()

        # other fields
        self.goal_index = 0
        self.executePathGoal = None
        self.executePathResult = ExecutePathResult()

    def handle_path(self, paramExecutePathGoal):
        '''
        Action server callback to handle following path in succession
        '''
        rospy.loginfo('handle_path')

        self.goal_index = 0
        self.executePathGoal = paramExecutePathGoal
        self.executePathResult = ExecutePathResult()

        if self.executePathGoal is not None:
            self.visualization_publisher.publish(self.executePathGoal.path)

        rate = rospy.Rate(10.0)
        while not rospy.is_shutdown():
            if not self.path_server.is_active():
                return

            if self.path_server.is_preempt_requested():
                rospy.loginfo('Preempt requested...')
                # Stop bug2
                self.goal_client.cancel_goal()
                # Stop path_server
                self.path_server.set_preempted()
                return

            if self.goal_index < len(self.executePathGoal.path.poses):
                moveto_goal = MoveToGoal()
                moveto_goal.target_pose = self.executePathGoal.path.poses[self.goal_index]
                self.goal_client.send_goal(moveto_goal, done_cb=self.handle_goal,
                                           feedback_cb=self.handle_goal_preempt)
                # Wait for result
                while self.goal_client.get_result() is None:
                    if self.path_server.is_preempt_requested():
                        self.goal_client.cancel_goal()
            else:
                rospy.loginfo('Done processing goals')
                self.goal_client.cancel_goal()
                self.path_server.set_succeeded(self.executePathResult, 'Done processing goals')
                return

            rate.sleep()
        self.path_server.set_aborted(self.executePathResult, 'Aborted. The node has been killed.')


    def handle_goal(self, state, result):
        '''
        Handle goal events (succeeded, preempted, aborted, unreachable, ...)
        Send feedback message
        '''
        feedback = ExecutePathFeedback()
        feedback.pose = self.executePathGoal.path.poses[self.goal_index]

        # state is GoalStatus message as shown here:
        # http://docs.ros.org/diamondback/api/actionlib_msgs/html/msg/GoalStatus.html
        if state == GoalStatus.SUCCEEDED:
            rospy.loginfo("Succeeded finding goal %d", self.goal_index + 1)
            self.executePathResult.visited.append(True)
            feedback.reached = True
        else:
            rospy.loginfo("Failed finding goal %d", self.goal_index + 1)
            self.executePathResult.visited.append(False)
            feedback.reached = False

            if not self.executePathGoal.skip_unreachable:
                rospy.loginfo('Failed finding goal %d, not skipping...', self.goal_index + 1)
#.........这里部分代码省略.........

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import set_preempted [as 别名]
class HokuyoLaserActionServer():
    def __init__(self):
        # Initialize constants
        self.error_threshold = 0.0175 # Report success if error reaches below threshold
        self.signal = 1
        
        # Initialize new node
        rospy.init_node(NAME, anonymous=True)
        
        controller_name = rospy.get_param('~controller')
        
        # Initialize publisher & subscriber for tilt
        self.laser_tilt = JointControllerState()
        self.laser_tilt_pub = rospy.Publisher(controller_name + '/command', Float64)
        self.laser_signal_pub = rospy.Publisher('laser_scanner_signal', LaserScannerSignal)
        self.joint_speed_srv = rospy.ServiceProxy(controller_name + '/set_speed', SetSpeed, persistent=True)
        
        rospy.Subscriber(controller_name + '/state', JointControllerState, self.process_tilt_state)
        rospy.wait_for_message(controller_name + '/state', JointControllerState)
        
        # Initialize tilt action server
        self.result = HokuyoLaserTiltResult()
        self.feedback = HokuyoLaserTiltFeedback()
        self.feedback.tilt_position = self.laser_tilt.process_value
        self.server = SimpleActionServer('hokuyo_laser_tilt_action', HokuyoLaserTiltAction, self.execute_callback)
        
        rospy.loginfo("%s: Ready to accept goals", NAME)

    def process_tilt_state(self, tilt_data):
        self.laser_tilt = tilt_data

    def reset_tilt_position(self, offset=0.0):
        self.laser_tilt_pub.publish(offset)
        rospy.sleep(0.5)

    def execute_callback(self, goal):
        r = rospy.Rate(100)
        self.joint_speed_srv(2.0)
        self.reset_tilt_position(goal.offset)
        delta = goal.amplitude - goal.offset
        target_speed = delta / goal.duration
        timeout_threshold = rospy.Duration(5.0) + rospy.Duration.from_sec(goal.duration)
        self.joint_speed_srv(target_speed)
        
        print "delta = %f, target_speed = %f" % (delta, target_speed)
        
        self.result.success = True
        self.result.tilt_position = self.laser_tilt.process_value
        rospy.loginfo("%s: Executing laser tilt %s time(s)", NAME, goal.tilt_cycles)
        
        # Tilt laser goal.tilt_cycles amount of times.
        for i in range(1, goal.tilt_cycles + 1):
            self.feedback.progress = i
            
            # Issue 2 commands for each cycle
            for j in range(2):
                if j % 2 == 0:
                    target_tilt = goal.offset + goal.amplitude     # Upper tilt limit
                    self.signal = 0
                else:
                    target_tilt = goal.offset     # Lower tilt limit
                    self.signal = 1
                    
                # Publish target command to controller
                self.laser_tilt_pub.publish(target_tilt)
                start_time = rospy.Time.now()
                current_time = start_time
                
                while abs(target_tilt - self.laser_tilt.process_value) > self.error_threshold:
                    #delta = abs(target_tilt - self.laser_tilt.process_value)
                    #time_left = goal.duration - (rospy.Time.now() - start_time).to_sec()
                    #target_speed = delta / time_left
                    #self.joint_speed_srv(target_speed)
                    
                    # Cancel exe if another goal was received (i.e. preempt requested)
                    if self.server.is_preempt_requested():
                        rospy.loginfo("%s: Aborted: Action Preempted", NAME)
                        self.result.success = False
                        self.server.set_preempted()
                        return
                        
                    # Publish current head position as feedback
                    self.feedback.tilt_position = self.laser_tilt.process_value
                    self.server.publish_feedback(self.feedback)
                    
                    # Abort if timeout
                    current_time = rospy.Time.now()
                    
                    if (current_time - start_time > timeout_threshold):
                        rospy.loginfo("%s: Aborted: Action Timeout", NAME)
                        self.result.success = False
                        self.server.set_aborted()
                        return
                        
                    r.sleep()
                    
                signal = LaserScannerSignal()
                signal.header.stamp = current_time
                signal.signal = self.signal
                self.laser_signal_pub.publish(signal)
#.........这里部分代码省略.........

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import set_preempted [as 别名]
class SmartArmGripperActionServer():

    def __init__(self):

        # Initialize constants
        self.JOINTS_COUNT = 2                           # Number of joints to manage
        self.ERROR_THRESHOLD = 0.01                     # Report success if error reaches below threshold
        self.TIMEOUT_THRESHOLD = rospy.Duration(5.0)    # Report failure if action does not succeed within timeout threshold

        # Initialize new node
        rospy.init_node(NAME, anonymous=True)

        # Initialize publisher & subscriber for left finger
        self.left_finger_frame = 'arm_left_finger_link'
        self.left_finger = JointControllerState(set_point=0.0, process_value=0.0, error=1.0)
        self.left_finger_pub = rospy.Publisher('finger_left_controller/command', Float64)
        rospy.Subscriber('finger_left_controller/state', JointControllerState, self.read_left_finger)
        rospy.wait_for_message('finger_left_controller/state', JointControllerState)

        # Initialize publisher & subscriber for right finger
        self.right_finger_frame = 'arm_right_finger_link'
        self.right_finger = JointControllerState(set_point=0.0, process_value=0.0, error=1.0)
        self.right_finger_pub = rospy.Publisher('finger_right_controller/command', Float64)
        rospy.Subscriber('finger_right_controller/state', JointControllerState, self.read_right_finger)
        rospy.wait_for_message('finger_right_controller/state', JointControllerState)

        # Initialize action server
        self.result = SmartArmGripperResult()
        self.feedback = SmartArmGripperFeedback()
        self.feedback.gripper_position = [self.left_finger.process_value, self.right_finger.process_value]
        self.server = SimpleActionServer(NAME, SmartArmGripperAction, self.execute_callback)

        # Reset gripper position
        rospy.sleep(1)
        self.reset_gripper_position()
        rospy.loginfo("%s: Ready to accept goals", NAME)


    def reset_gripper_position(self):
        self.left_finger_pub.publish(0.0)
        self.right_finger_pub.publish(0.0)
        rospy.sleep(5)


    def read_left_finger(self, data):
        self.left_finger = data
        self.has_latest_left_finger = True


    def read_right_finger(self, data):
        self.right_finger = data
        self.has_latest_right_finger = True


    def wait_for_latest_controller_states(self, timeout):
        self.has_latest_left_finger = False
        self.has_latest_right_finger = False
        r = rospy.Rate(100)
        start = rospy.Time.now()
        while (self.has_latest_left_finger == False or self.has_latest_right_finger == False) and \
                (rospy.Time.now() - start < timeout):
            r.sleep()


    def execute_callback(self, goal):
        r = rospy.Rate(100)
        self.result.success = True
        self.result.gripper_position = [self.left_finger.process_value, self.right_finger.process_value]
        rospy.loginfo("%s: Executing move gripper", NAME)
        
        # Initialize target joints
        target_joints = list()
        for i in range(self.JOINTS_COUNT):
            target_joints.append(0.0)

        # Retrieve target joints from goal
        if (len(goal.target_joints) > 0):
            for i in range(min(len(goal.target_joints), len(target_joints))):
                target_joints[i] = goal.target_joints[i] 
        else:
            rospy.loginfo("%s: Aborted: Invalid Goal", NAME)
            self.result.success = False
            self.server.set_aborted()
            return

        # Publish goal to controllers
        self.left_finger_pub.publish(target_joints[0])
        self.right_finger_pub.publish(target_joints[1])

        # Initialize loop variables
        start_time = rospy.Time.now()

        while (math.fabs(target_joints[0] - self.left_finger.process_value) > self.ERROR_THRESHOLD or \
                math.fabs(target_joints[1] - self.right_finger.process_value) > self.ERROR_THRESHOLD):
		
	        # Cancel exe if another goal was received (i.e. preempt requested)
            if self.server.is_preempt_requested():
                rospy.loginfo("%s: Aborted: Action Preempted", NAME)
                self.result.success = False
                self.server.set_preempted()
#.........这里部分代码省略.........

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import set_preempted [as 别名]
class MotionService(object):
    def __init__(self):
        rospy.init_node('motion_service')
        # Load config
        config_loader = RobotConfigLoader()
        try:
            robot_config_name = rospy.get_param(rospy.get_name() + '/robot_config')
        except KeyError:
            rospy.logwarn('Could not find robot config param in /' + rospy.get_name +'/robot_config. Using default config for '
                          'Thor Mang.')
            robot_config_name = 'thor'
        config_loader.load_xml_by_name(robot_config_name + '_config.xml')

        # Create publisher for first target
        if len(config_loader.targets) > 0:
            self._motion_publisher = MotionPublisher(
                config_loader.robot_config, config_loader.targets[0].joint_state_topic, config_loader.targets[0].publisher_prefix)
        else:
            rospy.logerr('Robot config needs to contain at least one valid target.')
        self._motion_data = MotionData(config_loader.robot_config)

        # Subscriber to start motions via topic
        self.motion_command_sub = rospy.Subscriber('motion_command', ExecuteMotionCommand, self.motion_command_request_cb)

        # Create action server
        self._action_server = SimpleActionServer(rospy.get_name() + '/motion_goal', ExecuteMotionAction, None, False)
        self._action_server.register_goal_callback(self._execute_motion_goal)
        self._action_server.register_preempt_callback(self._preempt_motion_goal)
        self._preempted = False

    def _execute_motion_goal(self):
        goal = self._action_server.accept_new_goal()
        # Check if motion exists
        if goal.motion_name not in self._motion_data:
            result = ExecuteMotionResult()
            result.error_code = [ExecuteMotionResult.MOTION_UNKNOWN]
            result.error_string = "The requested motion is unknown."
            self._action_server.set_aborted(result)
            return

        # check if a valid time_factor is set, otherwise default to 1.0
        if goal.time_factor <= 0.0:
            goal.time_factor = 1.0

        self._preempted = False

        self._motion_publisher.publish_motion(self._motion_data[goal.motion_name], goal.time_factor, self._trajectory_finished)
        print '[MotionService] New action goal received.'

    def _preempt_motion_goal(self):
        self._motion_publisher.stop_motion()
        print '[MotionService] Preempting goal.'
        self._preempted = True

    def _trajectory_finished(self, error_codes, error_groups):
        result = ExecuteMotionResult()
        result.error_code = error_codes
        error_string = ""
        for error_code, error_group in zip(error_codes, error_groups):
            error_string += error_group + ': ' + str(error_code) + '\n'
        result.error_string = error_string
        if self._preempted:
            self._action_server.set_preempted(result)
        else:
            self._action_server.set_succeeded(result)
        print '[MotionService] Trajectory finished with error code: \n', error_string

    def _execute_motion(self, request):
        response = ExecuteMotionResponse()

        # check if a motion with this name exists
        if request.motion_name not in self._motion_data:
            print '[MotionService] Unknown motion name: "%s"' % request.motion_name

            response.ok = False
            response.finish_time.data = rospy.Time.now()
            return response

        # check if a valid time_factor is set, otherwise default to 1.0
        if request.time_factor <= 0.0:
            request.time_factor = 1.0

        # find the duration of the requested motion
        motion_duration = 0.0
        for poses in self._motion_data[request.motion_name].values():
            if len(poses) > 0:
                endtime = poses[-1]['starttime'] + poses[-1]['duration']
                motion_duration = max(motion_duration, endtime)
        motion_duration *= request.time_factor

        # execute motion
        print '[MotionService] Executing motion: "%s" (time factor: %.3f, duration %.2fs)' % (request.motion_name, request.time_factor, motion_duration)
        self._motion_publisher.publish_motion(self._motion_data[request.motion_name], request.time_factor)

        # reply with ok and the finish_time of this motion
        response.ok = True
        response.finish_time.data = rospy.Time.now() + rospy.Duration.from_sec(motion_duration)
        return response

    def start_server(self):
#.........这里部分代码省略.........

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import set_preempted [as 别名]
class DropObjectActionServer:
    def __init__(self):
        self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus)
        self.start_audio_recording_srv = rospy.ServiceProxy('/audio_dump/start_audio_recording', StartAudioRecording)
        self.stop_audio_recording_srv = rospy.ServiceProxy('/audio_dump/stop_audio_recording', StopAudioRecording)
        
        self.posture_controller = SimpleActionClient('/wubble_gripper_grasp_action', GraspHandPostureExecutionAction)
        self.attached_object_pub = rospy.Publisher('/attached_collision_object', AttachedCollisionObject)
        self.action_server = SimpleActionServer(ACTION_NAME,
                                                DropObjectAction,
                                                execute_cb=self.drop_object,
                                                auto_start=False)

    def initialize(self):
        rospy.loginfo('%s: waiting for wubble_grasp_status service' % ACTION_NAME)
        rospy.wait_for_service('/wubble_grasp_status')
        rospy.loginfo('%s: connected to wubble_grasp_status service' % ACTION_NAME)
        
        rospy.loginfo('%s: waiting for wubble_gripper_grasp_action' % ACTION_NAME)
        self.posture_controller.wait_for_server()
        rospy.loginfo('%s: connected to wubble_gripper_grasp_action' % ACTION_NAME)
        
        rospy.loginfo('%s: waiting for audio_dump/start_audio_recording service' % ACTION_NAME)
        rospy.wait_for_service('audio_dump/start_audio_recording')
        rospy.loginfo('%s: connected to audio_dump/start_audio_recording service' % ACTION_NAME)
        
        rospy.loginfo('%s: waiting for audio_dump/stop_audio_recording service' % ACTION_NAME)
        rospy.wait_for_service('audio_dump/stop_audio_recording')
        rospy.loginfo('%s: connected to audio_dump/stop_audio_recording service' % ACTION_NAME)
        
        self.action_server.start()

    def open_gripper(self):
        pg = GraspHandPostureExecutionGoal()
        pg.goal = GraspHandPostureExecutionGoal.RELEASE
        
        self.posture_controller.send_goal(pg)
        self.posture_controller.wait_for_result()

    def drop_object(self, goal):
        if self.action_server.is_preempt_requested():
            rospy.loginfo('%s: preempted' % ACTION_NAME)
            self.action_server.set_preempted()
            
        # check that we have something in hand before dropping it
        grasp_status = self.get_grasp_status_srv()
        
        # if the object is still in hand after lift is done we are good
        if not grasp_status.is_hand_occupied:
            rospy.logerr('%s: gripper empty, nothing to drop' % ACTION_NAME)
            self.action_server.set_aborted()
            return
            
        # record sound padded by 0.5 seconds from start and 1.5 seconds from the end
        self.start_audio_recording_srv(InfomaxAction.DROP, goal.category_id)
        rospy.sleep(0.5)
        self.open_gripper()
        rospy.sleep(1.5)
        
        # check if gripper actually opened first
        sound_msg = None
        grasp_status = self.get_grasp_status_srv()
        
        # if there something in the gripper - drop failed
        if grasp_status.is_hand_occupied:
            self.stop_audio_recording_srv(False)
        else:
            sound_msg = self.stop_audio_recording_srv(True)
            
        # delete the object that we just dropped, we don't really know where it will land
        obj = AttachedCollisionObject()
        obj.object.header.stamp = rospy.Time.now()
        obj.object.header.frame_id = GRIPPER_LINK_FRAME
        obj.object.operation.operation = CollisionObjectOperation.REMOVE
        obj.object.id = goal.collision_object_name
        obj.link_name = GRIPPER_LINK_FRAME
        obj.touch_links = GRIPPER_LINKS
        
        self.attached_object_pub.publish(obj)
        
        if sound_msg:
            self.action_server.set_succeeded(DropObjectResult(sound_msg.recorded_sound))
        else:
            self.action_server.set_aborted()

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import set_preempted [as 别名]
class ShakePitchObjectActionServer:
    def __init__(self):
        self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus)
        self.start_audio_recording_srv = rospy.ServiceProxy('/audio_dump/start_audio_recording', StartAudioRecording)
        self.stop_audio_recording_srv = rospy.ServiceProxy('/audio_dump/stop_audio_recording', StopAudioRecording)
        self.wrist_pitch_velocity_srv = rospy.ServiceProxy('/wrist_pitch_controller/set_velocity', SetVelocity)
        
        self.wrist_pitch_command_pub = rospy.Publisher('wrist_pitch_controller/command', Float64)
        self.action_server = SimpleActionServer(ACTION_NAME,
                                                ShakePitchObjectAction,
                                                execute_cb=self.shake_pitch_object,
                                                auto_start=False)

    def initialize(self):
        rospy.loginfo('%s: waiting for wubble_grasp_status service' % ACTION_NAME)
        rospy.wait_for_service('/wubble_grasp_status')
        rospy.loginfo('%s: connected to wubble_grasp_status service' % ACTION_NAME)
        
        rospy.loginfo('%s: waiting for audio_dump/start_audio_recording service' % ACTION_NAME)
        rospy.wait_for_service('audio_dump/start_audio_recording')
        rospy.loginfo('%s: connected to audio_dump/start_audio_recording service' % ACTION_NAME)
        
        rospy.loginfo('%s: waiting for audio_dump/stop_audio_recording service' % ACTION_NAME)
        rospy.wait_for_service('audio_dump/stop_audio_recording')
        rospy.loginfo('%s: connected to audio_dump/stop_audio_recording service' % ACTION_NAME)
        
        rospy.loginfo('%s: waiting for wrist_pitch_controller service' % ACTION_NAME)
        rospy.wait_for_service('/wrist_pitch_controller/set_velocity')
        rospy.loginfo('%s: connected to wrist_pitch_controller service' % ACTION_NAME)
        
        self.action_server.start()

    def shake_pitch_object(self, goal):
        if self.action_server.is_preempt_requested():
            rospy.loginfo('%s: preempted' % ACTION_NAME)
            self.action_server.set_preempted()
            
        wrist_pitch_state = '/wrist_pitch_controller/state'
        desired_velocity = 6.0
        distance = 1.0
        threshold = 0.1
        timeout = 2.0
        
        try:
            msg = rospy.wait_for_message(wrist_pitch_state, DynamixelJointState, timeout)
            current_pos = msg.position
            start_pos = current_pos
            
            # set wrist to initial position
            self.wrist_pitch_velocity_srv(3.0)
            self.wrist_pitch_command_pub.publish(distance)
            end_time = rospy.Time.now() + rospy.Duration(timeout)
            
            while current_pos < distance-threshold and rospy.Time.now() < end_time:
                msg = rospy.wait_for_message(wrist_pitch_state, DynamixelJointState, timeout)
                current_pos = msg.position
                rospy.sleep(10e-3)
                
            self.wrist_pitch_velocity_srv(desired_velocity)
            
            # start recording sound and shaking
            self.start_audio_recording_srv(InfomaxAction.SHAKE_PITCH, goal.category_id)
            rospy.sleep(0.5)
            
            for i in range(2):
                self.wrist_pitch_command_pub.publish(-distance)
                end_time = rospy.Time.now() + rospy.Duration(timeout)
                
                while current_pos > -distance+threshold and rospy.Time.now() < end_time:
                    msg = rospy.wait_for_message(wrist_pitch_state, DynamixelJointState, timeout)
                    current_pos = msg.position
                    rospy.sleep(10e-3)
                    
                self.wrist_pitch_command_pub.publish(distance)
                end_time = rospy.Time.now() + rospy.Duration(timeout)
                
                while current_pos < distance-threshold and rospy.Time.now() < end_time:
                    msg = rospy.wait_for_message(wrist_pitch_state, DynamixelJointState, timeout)
                    current_pos = msg.position
                    rospy.sleep(10e-3)
                    
            rospy.sleep(0.5)
            
            # check if are still holding an object after shaking
            sound_msg = None
            grasp_status = self.get_grasp_status_srv()
            
            if grasp_status.is_hand_occupied:
                sound_msg = self.stop_audio_recording_srv(True)
            else:
                self.stop_audio_recording_srv(False)
                
            # reset wrist to starting position
            self.wrist_pitch_velocity_srv(3.0)
            self.wrist_pitch_command_pub.publish(start_pos)
            end_time = rospy.Time.now() + rospy.Duration(timeout)
            
            while current_pos < distance-threshold and rospy.Time.now() < end_time:
                msg = rospy.wait_for_message(wrist_pitch_state, DynamixelJointState, timeout)
                current_pos = msg.position
#.........这里部分代码省略.........