Python actionlib.SimpleActionServer类代码示例

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()

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)  

	def __init__(self):

		self.node_name = "PickAndPlaceServer"
		rospy.loginfo("Initalizing PickAndPlaceServer...")
		self.sg = SphericalGrasps()

		# Get the object size
		self.object_height = 0.1
		self.object_width = 0.05
		self.object_depth = 0.05
		self.pick_pose = rospy.get_param('~pickup_marker_pose')
		self.place_pose = rospy.get_param('~place_marker_pose')

		rospy.loginfo("%s: Waiting for pickup action server...", self.node_name)
		self.pickup_ac = SimpleActionClient('/pickup', PickupAction)
		connected = self.pickup_ac.wait_for_server(rospy.Duration(3000))
		if not connected:
			rospy.logerr("%s: Could not connect to pickup action server", self.node_name)
			exit()
		rospy.loginfo("%s: Connected to pickup action server", self.node_name)

		rospy.loginfo("%s: Waiting for place action server...", self.node_name)
		self.place_ac = SimpleActionClient('/place', PlaceAction)
		if not self.place_ac.wait_for_server(rospy.Duration(3000)):
			rospy.logerr("%s: Could not connect to place action server", self.node_name)
			exit()
		rospy.loginfo("%s: Connected to place action server", self.node_name)

		self.scene = PlanningSceneInterface()
		rospy.loginfo("Connecting to /get_planning_scene service")
		self.scene_srv = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene)
		self.scene_srv.wait_for_service()
		rospy.loginfo("Connected.")

		rospy.loginfo("Connecting to clear octomap service...")
		self.clear_octomap_srv = rospy.ServiceProxy('/clear_octomap', Empty)
		self.clear_octomap_srv.wait_for_service()
		rospy.loginfo("Connected!")

		# Get the links of the end effector exclude from collisions
		self.links_to_allow_contact = rospy.get_param('~links_to_allow_contact', None)
		if self.links_to_allow_contact is None:
			rospy.logwarn("Didn't find any links to allow contacts... at param ~links_to_allow_contact")
		else:
			rospy.loginfo("Found links to allow contacts: " + str(self.links_to_allow_contact))

		self.pick_as = SimpleActionServer(self.pick_pose, PickUpPoseAction,
			execute_cb=self.pick_cb, auto_start=False)
		self.pick_as.start()

		self.place_as = SimpleActionServer(self.place_pose, PickUpPoseAction,
			execute_cb=self.place_cb, auto_start=False)
		self.place_as.start()

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()

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.')

    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 __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 __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 __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 __init__(self):
        rospy.init_node('motion_service')
        # Load config
        config_loader = RobotConfigLoader()
        try:
            robot_config_name = rospy.get_param(rospy.get_name() + '/robot_config')
        except KeyError:
            rospy.logwarn('Could not find robot config param in /' + rospy.get_name +'/robot_config. Using default config for '
                          'Thor Mang.')
            robot_config_name = 'thor'
        config_loader.load_xml_by_name(robot_config_name + '_config.xml')

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

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

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

 def __init__(self, name):
     self.fullname = name
     self.jointPositions = []
     self.jointVelocities = []
     self.jointAccelerations = []
     self.jointNamesFromConfig = []
     for configJoint in configJoints:
         self.jointNamesFromConfig.append(configJoint['name'])
         self.jointPositions.append(0.0)
         self.jointVelocities.append(0.0)
         self.jointAccelerations.append(0.0)
         if 'mimic_joints' in configJoint:
             for mimic in configJoint['mimic_joints']:
                 self.jointNamesFromConfig.append(mimic['name'])
                 self.jointPositions.append(0.0)
                 self.jointVelocities.append(0.0)
                 self.jointAccelerations.append(0.0)
     self.pointsQueue = []
     self.lastTimeFromStart = 0.0
     startPositions = [0.0, -0.9, -0.9, 0.0, 0.0, 0.0]
     startVelocities = [0.5]*6
     self.jointPositions[1] = -0.9
     dynamixelChain.move_angles_sync(ids[:6], startPositions, startVelocities)
     # self.joint_state_pub = rospy.Publisher('/joint_states', JointState)
     self.server = SimpleActionServer(self.fullname,
                                      FollowJointTrajectoryAction,
                                      execute_cb=self.execute_cb,
                                      auto_start=False)
     self.server.start()

    def __init__(self):
        self.object_presence_pressure_threshold = rospy.get_param('object_presence_pressure_threshold', 200.0)
        self.object_presence_opening_threshold = rospy.get_param('object_presence_opening_threshold', 0.02)
        
        gripper_action_name = rospy.get_param('gripper_action_name', 'wubble_gripper_command_action')
        self.gripper_action_client = SimpleActionClient('wubble_gripper_action', WubbleGripperAction)
#        
#        while not rospy.is_shutdown():
#            try:
#                self.gripper_action_client.wait_for_server(timeout=rospy.Duration(2.0))
#                break
#            except ROSException as e:
#                rospy.loginfo('Waiting for %s action' % gripper_action_name)
#            except:
#                rospy.logerr('Unexpected error')
#                raise
                
        rospy.loginfo('Using gripper action client on topic %s' % gripper_action_name)
        
        query_service_name = rospy.get_param('grasp_query_name', 'wubble_grasp_status')
        self.query_srv = rospy.Service(query_service_name, GraspStatus, self.process_grasp_status)
        
        posture_action_name = rospy.get_param('posture_action_name', 'wubble_gripper_grasp_action')
        self.action_server = SimpleActionServer(posture_action_name, GraspHandPostureExecutionAction, self.process_grasp_action, False)
        self.action_server.start()
        
        rospy.loginfo('wubble_gripper grasp hand posture action server started on topic %s' % posture_action_name)

    def __init__(self):

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

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

        # Initialize publisher & subscriber for shoulder pan
        self.shoulder_pan_frame = 'arm_shoulder_tilt_link'
        self.shoulder_pan = JointState(set_point=0.0, process_value=0.0, error=1.0)
        self.shoulder_pan_pub = rospy.Publisher('shoulder_pan_controller/command', Float64)
        rospy.Subscriber('shoulder_pan_controller/state', JointState, self.read_shoulder_pan)
        rospy.wait_for_message('shoulder_pan_controller/state', JointState)

        # Initialize publisher & subscriber for arm tilt
        self.arm_tilt_frame = 'arm_pan_tilt_bracket'
        self.arm_tilt = JointState(set_point=0.0, process_value=0.0, error=1.0)
        self.arm_tilt_pub = rospy.Publisher('arm_tilt_controller/command', Float64)
        rospy.Subscriber('arm_tilt_controller/state', JointState, self.read_arm_tilt)
        rospy.wait_for_message('arm_tilt_controller/state', JointState)

        # Initialize publisher & subscriber for elbow tilt
        self.elbow_tilt_frame = 'arm_bracket'
        #self.elbow_tilt = JointState(set_point=0.0, process_value=0.0, error=1.0)
        self.elbow_tilt_pub = rospy.Publisher('elbow_tilt_controller/command', Float64)
        rospy.Subscriber('elbow_tilt_controller/state', JointState, self.read_elbow_tilt)
        rospy.wait_for_message('elbow_tilt_controller/state', JointState)

        # Initialize publisher & subscriber for wrist pan
        self.wrist_pan_frame = 'wrist_pan_link'
        self.wrist_pan = JointState(set_point=0.0, process_value=0.0, error=1.0)
        self.wrist_pan_pub = rospy.Publisher('wrist_pan_controller/command', Float64)
        rospy.Subscriber('wrist_pan_controller/state', JointState, self.read_wrist_pan)
        rospy.wait_for_message('wrist_pan_controller/state', JointState)

        # Initialize publisher & subscriber for wrist tilt
        self.wrist_tilt_frame = 'wrist_tilt_link'
        self.wrist_tilt = JointState(set_point=0.0, process_value=0.0, error=1.0)
        self.wrist_tilt_pub = rospy.Publisher('wrist_tilt_controller/command', Float64)
        rospy.Subscriber('wrist_tilt_controller/state', JointState, self.read_wrist_tilt)
        rospy.wait_for_message('wrist_tilt_controller/state', JointState)

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

        # Initialize joints action server
        self.result = RobbieArmResult()
        self.feedback = RobbieArmFeedback()
        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 = SimpleActionServer(NAME, RobbieArmAction, self.execute_callback)

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

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)

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()