Python SimpleActionServer.start方法代码示例

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import start [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 start [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 start [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 start [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 start [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 start [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 start [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 start [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 start [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 start [as 别名]
class Kill():
  REFRESH_RATE = 20

  def __init__(self):
    self.r1 = [-1.2923559122018107, -0.24199198117104131, -1.6400091364915879, -1.5193418228083817, 182.36402145110227, -0.18075144121090148, -5.948327320167482]
    self.r2 = [-0.6795931033163289, -0.22651111024292614, -1.748569353944001, -0.7718906399352281, 182.36402145110227, -0.18075144121090148, -5.948327320167482]
    self.r3 = [-0.2760036900225221, -0.12322070913238689, -1.5566246267792472, -0.7055856541215724, 182.30397617484758, -1.1580488044879909, -6.249409047256675]

    self.l1 = [1.5992829923087575, -0.10337038946934723, 1.5147248511783875, -1.554810647097346, 6.257580605941875, -0.13119498120772766, -107.10011839122919]
    self.l2 = [0.8243548398730115, -0.10751554070146568, 1.53941949443935, -0.7765233026995009, 6.257580605941875, -0.13119498120772766, -107.10011839122919]
    self.l3 = [0.31464495636226303, -0.06335699084094017, 1.2294536150663598, -0.7714563278010775, 6.730191306327954, -1.1396012223560352, -107.19066045395644]

    self.v = [0] * len(self.r1)

    self.r_joint_names = ['r_shoulder_pan_joint',
                          'r_shoulder_lift_joint',
                          'r_upper_arm_roll_joint',
                          'r_elbow_flex_joint',
                          'r_forearm_roll_joint',
                          'r_wrist_flex_joint',
                          'r_wrist_roll_joint']
    self.l_joint_names = ['l_shoulder_pan_joint',
                          'l_shoulder_lift_joint',
                          'l_upper_arm_roll_joint',
                          'l_elbow_flex_joint',
                          'l_forearm_roll_joint',
                          'l_wrist_flex_joint',
                          'l_wrist_roll_joint']

    self._action_name = 'kill'
    self._tf = tf.TransformListener()
    
    #initialize base controller
    topic_name = '/base_controller/command'
    self._base_publisher = rospy.Publisher(topic_name, Twist)

    #Initialize the sound controller
    self._sound_client = SoundClient()

    # Create a trajectory action client
    r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
    self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction)
    rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...')
    self.r_traj_action_client.wait_for_server()

    l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
    self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction)
    rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...')
    self.l_traj_action_client.wait_for_server()

    self.pose = None
    self._marker_sub = rospy.Subscriber('catch_me_destination_publisher', AlvarMarker, self.marker_callback)

    #initialize this client
    self._as = SimpleActionServer(self._action_name, KillAction, execute_cb=self.run, auto_start=False)
    self._as.start()
    rospy.loginfo('%s: started' % self._action_name)

  def marker_callback(self, marker):
    if (self.pose is not None):
      self.pose = marker.pose

  def run(self, goal):
    rospy.loginfo('Begin kill')
    self.pose = goal.pose
    #pose = self._tf.transformPose('/base_link', goal.pose)
    self._sound_client.say('Halt!')

    # turn to face the marker before opening arms (code repeated later)
    r = rospy.Rate(self.REFRESH_RATE)
    while(True):
      pose = self.pose
      if 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
      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)
#.........这里部分代码省略.........

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import start [as 别名]
class OnlineBagger(object):
    BAG_TOPIC = '/online_bagger/bag'

    def __init__(self):
        """
        Make dictionary of dequeues.
        Subscribe to set of topics defined by the yaml file in directory
        Stream topics up to a given stream time, dump oldest messages when limit is reached
        Set up service to bag n seconds of data default to all of available data
        """

        self.successful_subscription_count = 0  # successful subscriptions
        self.iteration_count = 0  # number of iterations
        self.streaming = True
        self.get_params()
        if len(self.subscriber_list) == 0:
            rospy.logwarn('No topics selected to subscribe to. Closing.')
            rospy.signal_shutdown('No topics to subscribe to')
            return
        self.make_dicts()

        self._action_server = SimpleActionServer(OnlineBagger.BAG_TOPIC, BagOnlineAction,
                                                 execute_cb=self.start_bagging, auto_start=False)
        self.subscribe_loop()
        rospy.loginfo('Remaining Failed Topics: {}\n'.format(
            self.get_subscriber_list(False)))
        self._action_server.start()

    def get_subscriber_list(self, status):
        """
        Get string of all topics, if their subscribe status matches the input (True / False)
        Outputs each topics: time_buffer(float in seconds), subscribe_statue(bool), topic(string)
        """
        sub_list = ''
        for topic in self.subscriber_list.keys():
            if self.subscriber_list[topic][1] == status:
                sub_list = sub_list + \
                    '\n{:13}, {}'.format(self.subscriber_list[topic], topic)
        return sub_list

    def get_params(self):
        """
        Retrieve parameters from param server.
        """
        self.dir = rospy.get_param('~bag_package_path', default=None)
        # Handle bag directory for MIL bag script
        if self.dir is None and 'BAG_DIR' in os.environ:
            self.dir = os.environ['BAG_DIR']

        self.stream_time = rospy.get_param(
            '~stream_time', default=30)  # seconds
        self.resubscribe_period = rospy.get_param(
            '~resubscribe_period', default=3.0)  # seconds
        self.dated_folder = rospy.get_param(
            '~dated_folder', default=True)  # bool

        self.subscriber_list = {}
        topics_param = rospy.get_param('~topics', default=[])

        # Add topics from rosparam to subscribe list
        for topic in topics_param:
            time = topic[1] if len(topic) == 2 else self.stream_time
            self.subscriber_list[topic[0]] = (time, False)

        def add_unique_topic(topic):
            if topic not in self.subscriber_list:
                self.subscriber_list[topic] = (self.stream_time, False)

        def add_env_var(var):
            for topic in var.split():
                add_unique_topic(topic)

        # Add topics from MIL bag script environment variables
        if 'BAG_ALWAYS' in os.environ:
            add_env_var(os.environ['BAG_ALWAYS'])
        for key in os.environ.keys():
            if key[0:4] == 'bag_':
                add_env_var(os.environ[key])

        rospy.loginfo(
            'Default stream_time: {} seconds'.format(self.stream_time))
        rospy.loginfo('Bag Directory: {}'.format(self.dir))

    def make_dicts(self):
        """
        Make dictionary with sliceable deques() that will be filled with messages and time stamps.

        Subscriber list contains all of the topics, their stream time and their subscription status:
        A True status for a given topic corresponds to a successful subscription
        A False status indicates a failed subscription.

        Stream time for an individual topic is specified in seconds.

        For Example:
        self.subscriber_list[0:1] = [['/odom', 300 ,False], ['/absodom', 300, True]]

        Indicates that '/odom' has not been subscribed to, but '/absodom' has been subscribed to

        self.topic_messages is a dictionary of deques containing a list of tuples.
        Dictionary Keys contain topic names
#.........这里部分代码省略.........

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import start [as 别名]
class Robot001Manager(object):
    def __init__(self, ip_robot):
        self.ip_robot = ip_robot
        # pan tilt
        self.joint_states = [None, None]
        self.js_pub = rospy.Publisher('/joint_states',
                                      JointState,
                                      queue_size=5)
        self.as_ = SimpleActionServer('/robot_controller',
                                      JointTrajectoryAction,
                                      auto_start=False,
                                      execute_cb=self.goal_cb)
        self.as_.start()
        self.go_to_position(pan=0.0, tilt=0.0)
        rospy.Timer(rospy.Duration(0.02), self.js_pub_cb, oneshot=False)
        rospy.loginfo("We are started!")

    def js_pub_cb(self, params):
        js = JointState()
        js.header.stamp = rospy.Time.now()
        js.name = ['pan_joint', 'tilt_joint']
        if self.joint_states[0] is None:
            return
        js.position = self.joint_states
        self.js_pub.publish(js)

    def goal_cb(self, goal):
        #goal = JointTrajectoryGoal()
        rospy.loginfo("Goal: " + str(goal))
        pan_idx = goal.trajectory.joint_names.index('pan_joint')
        tilt_idx = goal.trajectory.joint_names.index('tilt_joint')
        for p in goal.trajectory.points:
            pan_pos = p.positions[pan_idx]
            tilt_pos = p.positions[tilt_idx]
            # self.go_to_position(pan=pan_pos, tilt=tilt_pos)
        self.as_.set_succeeded(JointTrajectoryResult())

    def go_to_position(self, pan, tilt):
        if pan >= 0.0:
            self.go_to_right(pan)
        elif pan < 0.0:
            self.go_to_left(pan)

        if tilt >= 0.0:
            self.go_to_up(tilt)
        elif tilt < 0.0:
            self.go_to_down(tilt)

    def update_joints(self, ret_text):
        result_d = re.search("Down=((\d+)\.(\d+))", ret_text)
        result_r = re.search("Right=((\d+)\.(\d+))", ret_text)
        try:
            pan = float(result_r.groups()[0])
            tilt = float(result_d.groups()[0])
            self.joint_states = [radians(pan), radians(tilt)]
        except AttributeError as e:
            rospy.logwarn("Attribute error when parsing: " + str(e))
            #self.go_to_position(pan=0.0, tilt=0.0)

    def go_to_left(self, qtty):
        if qtty < 0.0:
            qtty = qtty * -1.0
        r = requests.get('http://' + self.ip_robot +
                         '?l=' + str(int(qtty)))
        self.update_joints(r.text)

    def go_to_right(self, qtty):
        if qtty < 0.0:
            qtty = qtty * -1.0
        r = requests.get('http://' + self.ip_robot +
                         '?r=' + str(int(qtty)))
        self.update_joints(r.text)

    def go_to_up(self, qtty):
        if qtty < 0.0:
            qtty = qtty * -1.0
        r = requests.get('http://' + self.ip_robot +
                         '?u=' + str(int(qtty)))
        self.update_joints(r.text)

    def go_to_down(self, qtty):
        if qtty < 0.0:
            qtty = qtty * -1.0
        r = requests.get('http://' + self.ip_robot +
                         '?d=' + str(int(qtty)))
        self.update_joints(r.text)

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

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

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

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

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