Python SimpleActionServer.set_succeeded方法代码示例

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import set_succeeded [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_succeeded [as 别名]
class axGripperServer:
    def __init__(self, name):
        self.fullname = name
        self.currentAngle = 0.0
        # dynamixelChain.move_angle(7, 0.0, 0.5)
        dynamixelChain.move_angles_sync(ids[6:], [0.0], [0.5])
        self.server = SimpleActionServer(self.fullname,
                                         GripperCommandAction,
                                         execute_cb=self.execute_cb,
                                         auto_start=False)
        self.server.start()

    def execute_cb(self, goal):
        rospy.loginfo(goal)
        self.currentAngle = goal.command.position
        dynamixelChain.move_angles_sync(ids[6:], [self.currentAngle], [0.5])
        # dynamixelChain.move_angle(7, 0.1, 0.5)
        rospy.sleep(0.1)
        print jPositions[4]
        attempts = 0
        while abs(goal.command.position - jPositions[4]) > 0.1 and attempts < 20:
            rospy.sleep(0.1)
            print jPositions[4]
            attempts += 1
        if attempts < 20:
            self.server.set_succeeded()
        else:
            self.server.set_aborted()

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import set_succeeded [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_succeeded [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_succeeded [as 别名]
class axGripperServer:
    def __init__(self, name):
        self.fullname = name
        self.goalAngle = 0.0
        self.actualAngle = 0.0
        self.failureState = False
        dynamixelChain.move_angles_sync(ids[6:], [self.goalAngle], [0.5])
        self.server = SimpleActionServer(
            self.fullname, GripperCommandAction, execute_cb=self.execute_cb, auto_start=False
        )
        self.server.start()

    def execute_cb(self, goal):
        rospy.loginfo(goal)
        self.currentAngle = goal.command.position
        dynamixelChain.move_angles_sync(ids[6:], [self.currentAngle], [0.5])
        attempts = 0
        for i in range(10):
            rospy.sleep(0.1)
            attempts += 1
        # while ... todo: add some condition to check on the actual angle
        #     rospy.sleep(0.1)
        #     print jPositions[4]
        #     attempts += 1
        if attempts < 20:
            self.server.set_succeeded()
        else:
            self.server.set_aborted()

    def checkFailureState(self):
        if self.failureState:
            print "I am currently in a failure state."

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import set_succeeded [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_succeeded [as 别名]
class ScanTableActionServer:
    """
      Scan Table Mock
      Run this file to have mock to the action '/head_traj_controller/head_scan_snapshot_action '(Scan Table)
    """
    def __init__(self):
        a_scan_table = {'name': '/head_traj_controller/head_scan_snapshot_action', 'ActionSpec': PointHeadAction, 'execute_cb': self.scan_table_cb, 'auto_start': False}
        self.s  = SimpleActionServer(**a_scan_table)
        self.s.start()

    def scan_table_cb(self, req):
         rospy.loginfo('Scan Table \'/head_traj_controller/head_scan_snapshot_action was called.')
         self.s.set_succeeded() if bool(random.randint(0, 1)) else self.s.set_aborted()

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import set_succeeded [as 别名]
class ybGripperServer:
    def __init__(self, name):
        self.fullname = name
        self.currentValue = 0.0
        gripperTopic = '/arm_1/gripper_controller/position_command'
        self.jppub = rospy.Publisher(gripperTopic, JointPositions)
        self.server = SimpleActionServer(self.fullname,
                                         GripperCommandAction,
                                         execute_cb=self.execute_cb,
                                         auto_start=False)
        self.server.start()

    def execute_cb(self, goal):
        rospy.loginfo(goal)
        self.currentValue = goal.command.position
        self.moveGripper(self.jppub, self.currentValue)
        attempts = 0
        # here we should be checking if the gripper has gotten to its goal
        for i in range(5):
            rospy.sleep(0.1)
            attempts += 1
        if attempts < 20:
            self.server.set_succeeded()
        else:
            self.server.set_aborted()

    def moveGripper(self, gPublisher, floatVal):
        jp = JointPositions()
        myPoison = Poison()
        myPoison.originator = 'yb_grip'
        myPoison.description = 'whoknows'
        myPoison.qos = 0.0
        jp.poisonStamp = myPoison
        nowTime = rospy.Time.now()
        jvl = JointValue()
        jvl.timeStamp = nowTime
        jvl.joint_uri = 'gripper_finger_joint_l'
        jvl.unit = 'm'
        jvl.value = floatVal
        jp.positions.append(jvl)
        jvr = JointValue()
        jvr.timeStamp = nowTime
        jvr.joint_uri = 'gripper_finger_joint_r'
        jvr.unit = 'm'
        jvr.value = floatVal
        jp.positions.append(jvr)
        gPublisher.publish(jp)

    def ybspin(self):
        rospy.sleep(0.1)

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import set_succeeded [as 别名]
class ReemTabletopManipulationMock():
    def __init__(self):

        a_grasp_target_action = {'name': '/tabletop_grasping_node', 'ActionSpec': GraspTargetAction, 'execute_cb': self.grasp_target_action_cb, 'auto_start': False}

        self.s  = SimpleActionServer(**a_grasp_target_action)
        self.s.start()


    def grasp_target_action_cb(self, req):
        rospy.loginfo('Grasp Target Action \'%s\' /tabletop_grasping_node was called.' % req.appearanceID)
        res = GraspTargetResult()
        res.detectionResult = TabletopDetectionResult()
        res.detectionResult.models = [DatabaseModelPoseList()]
        res.detectionResult.models[0].model_list = [DatabaseModelPose()]# = [0].model_id = req.databaseID
        res.detectionResult.models[0].model_list[0].model_id = req.databaseID
        self.s.set_succeeded(res) if bool(random.randint(0, 1)) else self.s.set_aborted(res)

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import set_succeeded [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_succeeded [as 别名]
class ConstructStaticCollisionMapServer:

    def __init__(self,
                 node_name = 'construct_static_collision_map_server',
                 action_name = 'construct_static_collision_map_action',
                 head_topic = '/head_traj_controller/point_head_action',
                 static_collision_map_topic = '/make_static_collision_map'
                 
                 ):
                 
        rospy.init_node(node_name)
        rospy.loginfo ( 'waiting for SimpleActionClient: %s'%( head_topic )  )
        self.head_client = SimpleActionClient(head_topic,PointHeadAction )
        self.head_client.wait_for_server( )

        self.take_static_collision_map_client = SimpleActionClient(static_collision_map_topic, MakeStaticCollisionMapAction)

        self._action_server = SimpleActionServer(action_name, ConstructStaticCollisionMapAction, execute_cb=self.execute_cb)
        rospy.loginfo('ready')

    def execute_cb(self, goal):

        self.scan_table()
        goal = MakeStaticCollisionMapGoal()
        self.take_static_collision_map_client.send_goal(goal)

        self._action_server.set_succeeded()

    def move_head(self, x, y, z,
                  min_dur = 0.0,
                  max_velocity = 1.0,
                  frame_id = 'base_link',
                  timeout = 5.0):
        point = PointStamped()
        point.header.frame_id = frame_id
        point.header.stamp = rospy.Time.now()
        point.point.x, point.point.y, point.point.z = x, y, z
            
        goal = PointHeadGoal()
        goal.pointing_frame = 'head_plate_frame'
        goal.max_velocity = max_velocity
        goal.min_duration = rospy.Duration.from_sec( min_dur )
        goal.target = point
        
        self.head_client.send_goal( goal )
        self.head_client.wait_for_result( timeout = 
                                          rospy.Duration.from_sec( timeout ) )
        if not self.head_client.get_state() == GoalStatus.SUCCEEDED:
            rospy.logerr( 'can not move head to:\n %s'%( goal ) )
            return False
            
        return True

    def scan_table(self,
                   x = 0.5, 
                   y_min = -1.0, y_max = 1.0, 
                   z = 0.7, 
                   steps = 10, 
                   min_dur = 1.0, 
                   max_wait_time = 10.0
                   ):
        rospy.loginfo( 'scanning table with x = %5.3f, y_min = %5.3f, y_max = %5.3f, z = %5.3f, steps = %d'%( x, y_min, y_max, z, steps ) )
        Y = np.linspace( y_min, y_max, steps )
        
        for y in Y:
        
            if not self.move_head( x, y, z, min_dur ):
                return

        rospy.loginfo( 'done scanning table!' )

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

#.........这里部分代码省略.........
        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
                rospy.sleep(10e-3)
                
            rospy.sleep(0.5)
            
            if sound_msg:
                self.action_server.set_succeeded(ShakePitchObjectResult(sound_msg.recorded_sound))
                return
            else:
                self.action_server.set_aborted()
                return
        except:
            rospy.logerr('%s: attempted pitch failed - %s' % ACTION_NAME)
            self.stop_audio_recording_srv(False)
            self.action_server.set_aborted()

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import set_succeeded [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_succeeded [as 别名]

#.........这里部分代码省略.........
        # 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)
                #rospy.sleep(0.5)
                
        if self.result.success:
            rospy.loginfo("%s: Goal Completed", NAME)
            self.result.tilt_position = self.laser_tilt.process_value
            self.server.set_succeeded(self.result)

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

#.........这里部分代码省略.........
      else:
        break

    # open arms
    traj_goal_r = JointTrajectoryGoal()
    traj_goal_r.trajectory.joint_names = self.r_joint_names
    traj_goal_l = JointTrajectoryGoal()
    traj_goal_l.trajectory.joint_names = self.l_joint_names
    traj_goal_r.trajectory.points.append(JointTrajectoryPoint(positions=self.r1, velocities = self.v, time_from_start = rospy.Duration(2)))
    self.r_traj_action_client.send_goal_and_wait(traj_goal_r, rospy.Duration(3))

    traj_goal_l.trajectory.points.append(JointTrajectoryPoint(positions=self.l1, velocities = self.v, time_from_start = rospy.Duration(2)))
    self.l_traj_action_client.send_goal_and_wait(traj_goal_l, rospy.Duration(3))

    traj_goal_r.trajectory.points[0].positions = self.r2
    self.r_traj_action_client.send_goal(traj_goal_r)

    traj_goal_l.trajectory.points[0].positions = self.l2
    self.l_traj_action_client.send_goal(traj_goal_l)

    self._sound_client.say('You have the right to remain silent.')

    # Keep a local copy because it will update
    #pose = self.pose
    #num_move_x = int((pose.pose.position.x - 0.3) * 10 / .1) + 1
    #print str(pose.pose.position.x) + ', ' + str(num_move_x)
    #twist_msg = Twist()
    #twist_msg.linear = Vector3(.1, 0.0, 0.0)
    #twist_msg.angular = Vector3(0.0, 0.0, 0.0)
    #for i in range(num_move_x):
    #  self._base_publisher.publish(twist_msg)
    #  r.sleep()

    # track the marker as much as we can
    while True:
      pose = self.pose
      # too far away
      if abs(pose.pose.position.x > 1.5):
        rospy.loginfo('Target out of range: ' + str(pose.pose.position.x))
        self._as.set_aborted()
        return;
      # too far to the side -> rotate
      elif abs(pose.pose.position.y) > .1:
        num_move_y = int((abs(pose.pose.position.y) - 0.1) * self.REFRESH_RATE / .33) + 1
        #print str(pose.pose.position.x) + ', ' + str(num_move_x)
        twist_msg = Twist()
        twist_msg.linear = Vector3(0.0, 0.0, 0.0)
        twist_msg.angular = Vector3(0.0, 0.0, pose.pose.position.y / (3 * abs(pose.pose.position.y)))
        for i in range(num_move_y):
          if pose != self.pose:
            break
          self._base_publisher.publish(twist_msg)
          r.sleep()
        pose.pose.position.y = 0
        #twist_msg = Twist()
        #twist_msg.linear = Vector3(0.0, 0.0, 0.0)
        #twist_msg.angular = Vector3(0.0, 0.0, pose.pose.position.y / (5 * abs(pose.pose.position.y)))
        #self._base_publisher.publish(twist_msg)

      # now we are going to move in for the kill, but only until .5 meters away (don't want to run suspect over)
      elif pose.pose.position.x > .5:
        num_move_x = int((pose.pose.position.x - 0.3) * self.REFRESH_RATE / .1) + 1
        #print str(pose.pose.position.x) + ', ' + str(num_move_x)
        twist_msg = Twist()
        twist_msg.linear = Vector3(.1, 0.0, 0.0)
        twist_msg.angular = Vector3(0.0, 0.0, 0.0)
        for i in range(num_move_x):
          if pose != self.pose:
            break
          self._base_publisher.publish(twist_msg)
          r.sleep()
        pose.pose.position.x = 0
        #twist_msg = Twist()
        #twist_msg.linear = Vector3(.1, 0.0, 0.0)
        #twist_msg.angular = Vector3(0.0, 0.0, 0.0)
        #self._base_publisher.publish(twist_msg)
     
      # susupect is within hugging range!!!
      else:
        break
      r.sleep()

    # after exiting the loop, we should be ready to capture -> send close arms goal
    self._sound_client.say("Anything you say do can and will be used against you in a court of law.")

    self.l_traj_action_client.wait_for_result(rospy.Duration(3))
    self.r_traj_action_client.wait_for_result(rospy.Duration(3))

    traj_goal_r.trajectory.points[0].positions = self.r3
    self.r_traj_action_client.send_goal(traj_goal_r)

    traj_goal_l.trajectory.points[0].positions = self.l3
    self.l_traj_action_client.send_goal(traj_goal_l)

    self.l_traj_action_client.wait_for_result(rospy.Duration(3))
    self.r_traj_action_client.wait_for_result(rospy.Duration(3))

    rospy.loginfo('End kill')
    rospy.sleep(20)
    self._as.set_succeeded()