Python SimpleActionServer.is_active方法代码示例

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

    def __init__(self):


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

        #initialize the clients to interface with 
        self.arm_client = SimpleActionClient("smart_arm_action", SmartArmAction)
        self.gripper_client = SimpleActionClient("smart_arm_gripper_action", SmartArmGripperAction)
        self.move_client = SimpleActionClient("erratic_base_action", ErraticBaseAction)
        
        self.move_client.wait_for_server()
        self.arm_client.wait_for_server()
        self.gripper_client.wait_for_server()

        # Initialize tf listener (remove?)
        self.tf = tf.TransformListener()

        # Initialize erratic base action server
        self.result = SmartArmGraspResult()
        self.feedback = SmartArmGraspFeedback()
        self.server = SimpleActionServer(NAME, SmartArmGraspAction, self.execute_callback)

        #define the offsets
    	# These offsets were determines expirmentally using the simulator
    	# They were tested using points stamped with /map
        self.xOffset = 0.025
        self.yOffset = 0.0
        self.zOffset = 0.12 #.05 # this does work!

        rospy.loginfo("%s: Pick up Action Server is ready to accept goals", NAME)
        rospy.loginfo("%s: Offsets are [%f, %f, %f]", NAME, self.xOffset, self.yOffset, self.zOffset )

    def move_to(self, frame_id, position, orientation, vicinity=0.0):
        goal = ErraticBaseGoal()
        goal.target_pose.header.stamp = rospy.Time.now()
        goal.target_pose.header.frame_id = frame_id
        goal.target_pose.pose.position = position
        goal.target_pose.pose.orientation = orientation
        goal.vicinity_range = vicinity
        self.move_client.send_goal(goal)
        #print "going into loop"
        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()

    #(almost) blatent copy / past from wubble_head_action.py.  Err, it's going to the wrong position
    def transform_target_point(self, point, frameId):
        
        #rospy.loginfo("%s: got %s %s %s %s", NAME, point.header.frame_id, point.point.x, point.point.y, point.point.z)

        # Wait for tf info (time-out in 5 seconds)
        self.tf.waitForTransform(frameId, point.header.frame_id, rospy.Time(), rospy.Duration(5.0))

        # Transform target point & retrieve the pan angle
        return self.tf.transformPoint(frameId, point)

#UNUSED
    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)

# This moves the arm into a positions based on angles (in rads)
# It depends on the sources code in wubble_actions
    def move_arm(self, shoulder_pan, shoulder_tilt, elbow_tilt, wrist_rotate):
        goal = SmartArmGoal()
        goal.target_joints = [shoulder_pan, shoulder_tilt, elbow_tilt, wrist_rotate]

        self.arm_client.send_goal(goal, None, None, self.arm_position_feedback_cb)
        self.arm_client.wait_for_goal_to_finish()

        while not self.arm_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.arm_client.cancel_goal()
                return GoalStatus.PREEMPTED

        return self.arm_client.get_state()

# This moves the wrist of the arm to the x, y, z coordinates
    def reach_at(self, frame_id, x, y, z):
	        
        goal = SmartArmGoal()
        goal.target_point = PointStamped()
        goal.target_point.header.frame_id = frame_id
        goal.target_point.point.x = x
        goal.target_point.point.y = y
        goal.target_point.point.z = z

       	rospy.loginfo("%s: Original point is '%s' [%f, %f, %f]", NAME, 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 is_active [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 is_active [as 别名]
class ErraticBaseActionServer():
    def __init__(self):
        self.base_frame = '/base_footprint'
        
        self.move_client = SimpleActionClient('move_base', MoveBaseAction)
        self.move_client.wait_for_server()
        
        self.tf = tf.TransformListener()
        
        self.result = ErraticBaseResult()
        self.feedback = ErraticBaseFeedback()
        self.server = SimpleActionServer(NAME, ErraticBaseAction, self.execute_callback, auto_start=False)
        self.server.start()
        
        rospy.loginfo("%s: Ready to accept goals", NAME)


    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)
#.........这里部分代码省略.........