Python SimpleActionServer.set_aborted方法代码示例

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

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

    def transform_target_point(self, point):
        self.tf.waitForTransform(self.base_frame, point.header.frame_id, rospy.Time(), rospy.Duration(5.0))
        return self.tf.transformPoint(self.base_frame, point)


    def move_to(self, target_pose):
        goal = MoveBaseGoal()
        goal.target_pose = target_pose
        goal.target_pose.header.stamp = rospy.Time.now()
        
        self.move_client.send_goal(goal=goal, feedback_cb=self.move_base_feedback_cb)
        
        while not self.move_client.wait_for_result(rospy.Duration(0.01)):
            # check for preemption
            if self.server.is_preempt_requested():
                rospy.loginfo("%s: Aborted: Action Preempted", NAME)
                self.move_client.cancel_goal()
                return GoalStatus.PREEMPTED
                
        return self.move_client.get_state()


    def move_base_feedback_cb(self, fb):
        self.feedback.base_position = fb.base_position
        if self.server.is_active():
            self.server.publish_feedback(self.feedback)


    def get_vicinity_target(self, target_pose, vicinity_range):
        vicinity_pose = PoseStamped()
        
        # transform target to base_frame reference
        target_point = PointStamped()
        target_point.header.frame_id = target_pose.header.frame_id
        target_point.point = target_pose.pose.position
        self.tf.waitForTransform(self.base_frame, target_pose.header.frame_id, rospy.Time(), rospy.Duration(5.0))
        target = self.tf.transformPoint(self.base_frame, target_point)
        
        rospy.logdebug("%s: Target at (%s, %s, %s)", NAME, target.point.x, target.point.y, target.point.z)
        
        # find distance to point
        dist = math.sqrt(math.pow(target.point.x, 2) + math.pow(target.point.y, 2))
        
        if (dist < vicinity_range):
            # if already within range, then no need to move
            vicinity_pose.pose.position.x = 0.0
            vicinity_pose.pose.position.y = 0.0
        else:
            # normalize vector pointing from source to target
            target.point.x /= dist
            target.point.y /= dist
            
            # scale normal vector to within vicinity_range distance from target
            target.point.x *= (dist - vicinity_range)
            target.point.y *= (dist - vicinity_range)
            
            # add scaled vector to source
            vicinity_pose.pose.position.x = target.point.x + 0.0
            vicinity_pose.pose.position.y = target.point.y + 0.0
            
        # set orientation
        ori = Quaternion()
        yaw = math.atan2(target.point.y, target.point.x)
        (ori.x, ori.y, ori.z, ori.w) = tf.transformations.quaternion_from_euler(0, 0, yaw)
        vicinity_pose.pose.orientation = ori
        
        # prep header
        vicinity_pose.header = target_pose.header
        vicinity_pose.header.frame_id = self.base_frame
        
        rospy.logdebug("%s: Moving to (%s, %s, %s)", NAME, vicinity_pose.pose.position.x, vicinity_pose.pose.position.y, vicinity_pose.pose.position.z)
        
        return vicinity_pose


    def execute_callback(self, goal):
        rospy.loginfo("%s: Executing move base", NAME)
        
        move_base_result = None
        
        if goal.vicinity_range == 0.0:
            # go to exactly
            move_base_result = self.move_to(goal.target_pose)
        else:
            # go near (within vicinity_range meters)
            vicinity_target_pose = self.get_vicinity_target(goal.target_pose, goal.vicinity_range)
            move_base_result = self.move_to(vicinity_target_pose)
            
        # check results
        if (move_base_result == GoalStatus.SUCCEEDED):
            rospy.loginfo("%s: Succeeded", NAME)
            self.result.base_position = self.feedback.base_position
            self.server.set_succeeded(self.result)
        elif (move_base_result == GoalStatus.PREEMPTED):
            rospy.loginfo("%s: Preempted", NAME)
            self.server.set_preempted()
        else:
            rospy.loginfo("%s: Aborted", NAME)
            self.server.set_aborted()

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

#.........这里部分代码省略.........
        Create ros bag save directory
        If no bag name is provided, the current date/time is used as default.
        """
        # If directory param is not set, default to $HOME/bags/<date>
        default_dir = self.dir
        if default_dir is None:
            default_dir = os.path.join(os.environ['HOME'], 'bags')

        # if dated folder param is set to True, append current date to
        # directory
        if self.dated_folder is True:
            default_dir = os.path.join(default_dir, str(datetime.date.today()))
        # Split filename from directory
        bag_dir, bag_name = os.path.split(filename)
        bag_dir = os.path.join(default_dir, bag_dir)
        if not os.path.exists(bag_dir):
            os.makedirs(bag_dir)

        # Create default filename if only directory specified
        if bag_name == '':
            bag_name = self._get_default_filename()
        # Make sure filename ends in .bag, add otherwise
        if bag_name[-4:] != '.bag':
            bag_name = bag_name + '.bag'
        return os.path.join(bag_dir, bag_name)

    def start_bagging(self, req):
        """
        Dump all data in dictionary to bags, temporarily stops streaming
        during the bagging process, resumes streaming when over.
        If bagging is already false because of an active call to this service
        """
        result = BagOnlineResult()
        if self.streaming is False:
            result.status = 'Bag Request came in while bagging, priority given to prior request'
            result.success = False
            self._action_server.set_aborted(result)
            return
        bag = None
        try:
            self.streaming = False
            result.filename = self.get_bag_name(req.bag_name)
            bag = rosbag.Bag(result.filename, 'w')

            requested_seconds = req.bag_time

            selected_topics = req.topics.split()

            feedback = BagOnlineFeedback()
            total_messages = 0
            bag_topics = {}
            for topic, (time, subscribed) in self.subscriber_list.iteritems():
                if not subscribed:
                    continue
                # Exclude topics that aren't in topics service argument
                # If topics argument is empty string, include all topics
                if len(selected_topics) > 0 and topic not in selected_topics:
                    continue
                if len(self.topic_messages[topic]) == 0:
                    continue

                if req.bag_time == 0:
                    index = 0
                else:
                    index = self.get_time_index(topic, requested_seconds)
                total_messages += len(self.topic_messages[topic][index:])
                bag_topics[topic] = index
            if total_messages == 0:
                result.success = False
                result.status = 'no messages'
                self._action_server.set_aborted(result)
                self.streaming = True
                bag.close()
                return
            self._action_server.publish_feedback(feedback)
            msg_inc = 0
            for topic, index in bag_topics.iteritems():
                for msgs in self.topic_messages[topic][index:]:
                    bag.write(topic, msgs[1], t=msgs[0])
                    if msg_inc % 50 == 0:  # send feedback every 50 messages
                        feedback.progress = float(msg_inc) / total_messages
                        self._action_server.publish_feedback(feedback)
                    msg_inc += 1
                    # empty deque when done writing to bag
                self.topic_messages[topic].clear()
            feedback.progress = 1.0
            self._action_server.publish_feedback(feedback)
            bag.close()
        except Exception as e:
            result.success = False
            result.status = 'Exception while writing bag: ' + str(e)
            self._action_server.set_aborted(result)
            self.streaming = True
            if bag is not None:
                bag.close()
            return
        rospy.loginfo('Bag written to {}'.format(result.filename))
        result.success = True
        self._action_server.set_succeeded(result)
        self.streaming = True

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

#.........这里部分代码省略.........
            rospy.logerr('%s: unable to find a feasable grasp, aborting' % ACTION_NAME)
            return None
            
        pose_stamped = PoseStamped()
        pose_stamped.header.frame_id = grasping_result.grasps[0].grasp_posture.header.frame_id
        pose_stamped.pose = grasping_result.grasps[0].grasp_pose
        
        rospy.loginfo('%s: found good grasp, looking for corresponding IK' % ACTION_NAME)
        
        return self.find_ik_for_grasping_pose(pose_stamped)


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


    def close_gripper(self):
        pg = GraspHandPostureExecutionGoal()
        pg.goal = GraspHandPostureExecutionGoal.GRASP
        
        self.posture_controller.send_goal(pg)
        self.posture_controller.wait_for_result()
        
        rospy.sleep(1)
        grasp_status = self.get_grasp_status_srv()
        return grasp_status.is_hand_occupied


    def grasp_object(self, goal):
        if self.action_server.is_preempt_requested():
            rospy.loginfo('%s: preempted' % ACTION_NAME)
            self.action_server.set_preempted()
            
        target = goal.graspable_object
        collision_object_name = goal.collision_object_name
        collision_support_surface_name = goal.collision_support_surface_name
        
        ik_solution = self.find_grasp_pose(target, collision_object_name, collision_support_surface_name)
        
        if ik_solution:
            # disable collisions between gripper and target object
            ordered_collision_operations = OrderedCollisionOperations()
            collision_operation1 = CollisionOperation()
            collision_operation1.object1 = collision_object_name
            collision_operation1.object2 = GRIPPER_GROUP_NAME
            collision_operation1.operation = CollisionOperation.DISABLE
            
            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.02
            
            ordered_collision_operations.collision_operations = [collision_operation1, collision_operation2]
            
            # set gripper padding to 0
            gripper_paddings = [LinkPadding(l,0.0) for l in GRIPPER_LINKS]
            gripper_paddings.extend([LinkPadding(l,0.0) for l in ARM_LINKS])
            
            self.open_gripper()
            
            # move into pre-grasp pose
            if not move_arm_joint_goal(self.move_arm_client,
                                       ik_solution.joint_state.name,
                                       ik_solution.joint_state.position,
                                       link_padding=gripper_paddings,
                                       collision_operations=ordered_collision_operations):
                rospy.logerr('%s: attempted grasp failed' % ACTION_NAME)
                self.action_server.set_aborted()
                return
                
            # record grasping sound with 0.5 second padding before and after
            self.start_audio_recording_srv(InfomaxAction.GRASP, goal.category_id)
            rospy.sleep(0.5)
            grasp_successful = self.close_gripper()
            rospy.sleep(0.5)
            
            # if grasp was successful, attach it to the gripper
            if grasp_successful:
                sound_msg = self.stop_audio_recording_srv(True)
                
                obj = AttachedCollisionObject()
                obj.object.header.stamp = rospy.Time.now()
                obj.object.header.frame_id = GRIPPER_LINK_FRAME
                obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
                obj.object.id = collision_object_name
                obj.link_name = GRIPPER_LINK_FRAME
                obj.touch_links = GRIPPER_LINKS
                
                self.attached_object_pub.publish(obj)
                self.action_server.set_succeeded(GraspObjectResult(sound_msg.recorded_sound))
                return 
                
        self.stop_audio_recording_srv(False)
        rospy.logerr('%s: attempted grasp failed' % ACTION_NAME)
        self.action_server.set_aborted()

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import set_aborted [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_aborted [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()
#.........这里部分代码省略.........