Python SimpleActionServer.is_preempt_requested方法代码示例

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import is_preempt_requested [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 is_preempt_requested [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 is_preempt_requested [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 is_preempt_requested [as 别名]
class LocalSearch():
  VISUAL_FIELD_SIZE = 40
  MIN_HEAD_ANGLE = -140
  MAX_HEAD_ANGLE = 140

  _feedback = LocalSearchFeedback()
  _result = LocalSearchResult()

  def __init__(self):
    self._action_name = 'local_search'
    self.found_marker = False
    self.tracking_started = False
    
    #initialize head mover
    name_space = '/head_traj_controller/point_head_action'
    self.head_client = SimpleActionClient(name_space, PointHeadAction)
    self.head_client.wait_for_server()
    rospy.loginfo('%s: Action client for PointHeadAction running' % self._action_name)

    #initialize tracker mark
    rospy.Subscriber('catch_me_destination_publisher', AlvarMarker, self.marker_tracker)
    rospy.loginfo('%s: subscribed to AlvarMarkers' % self._action_name)
    
    #initialize this client
    self._as = SimpleActionServer(self._action_name, LocalSearchAction, execute_cb=self.run, auto_start=False)
    self._as.start()
    rospy.loginfo('%s: started' % self._action_name)
    
  def marker_tracker(self, marker):
    if self.tracking_started:
      self.found_marker = True  
      rospy.loginfo('%s: marker found' % self._action_name)
    
  def run(self, goal):
    success = False
    self.tracking_started = True
    self.found_marker = False

    rospy.loginfo('%s: Executing search for AR marker' % self._action_name)

    # define a set of ranges to search
    search_ranges = [
      # first search in front of the robot
      (0, self.VISUAL_FIELD_SIZE),
      (self.VISUAL_FIELD_SIZE, -self.VISUAL_FIELD_SIZE),
      # then search all directions
      (-self.VISUAL_FIELD_SIZE, self.MAX_HEAD_ANGLE),
      (self.MAX_HEAD_ANGLE, self.MIN_HEAD_ANGLE),
      (self.MIN_HEAD_ANGLE, 0)
    ]
    range_index = 0

    #success = self.search_range(*(search_ranges[range_index]))
    
    while (not success) and range_index < len(search_ranges) - 1:
      if self._as.is_preempt_requested():
        rospy.loginfo('%s: Premepted' % self._action_name)
        self._as.set_preempted()
        break
      range_index = range_index + 1
      success = self.search_range(*(search_ranges[range_index]))
    

    if success:
      rospy.loginfo('%s: Succeeded' % self._action_name)
      self._as.set_succeeded()
    else:
      self._as.set_aborted()
    self.tracking_started = False
    
  def search_range(self, start_angle, end_angle):
    rospy.loginfo("{}: searching range {} {}".format(self._action_name, start_angle, end_angle))
    angle_tick = self.VISUAL_FIELD_SIZE if (start_angle < end_angle) else -self.VISUAL_FIELD_SIZE
    for cur_angle in xrange(start_angle, end_angle, angle_tick):
      if self._as.is_preempt_requested():
	return False
      
      head_goal = self.lookat_goal(cur_angle)
      rospy.loginfo('%s: Head move goal for %s: %s produced' % (self._action_name, str(cur_angle), str(head_goal)))
      self.head_client.send_goal(head_goal)
      self.head_client.wait_for_result(rospy.Duration.from_sec(5.0))
      if (self.head_client.get_state() != GoalStatus.SUCCEEDED):
        rospy.logwarn('Head could not move to specified location')
        break

      # pause at each tick
      rospy.sleep(0.3)
      if (self.found_marker):
        # found a marker!
        return True

    # no marker found
    return False

  def lookat_goal(self, angle):
    head_goal = PointHeadGoal()
    head_goal.target.header.frame_id = '/torso_lift_link'
    head_goal.max_velocity = 0.8

    angle_in_radians = math.radians(angle)
#.........这里部分代码省略.........

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import is_preempt_requested [as 别名]
class ShakePitchObjectActionServer:
    def __init__(self):
        self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus)
        self.start_audio_recording_srv = rospy.ServiceProxy('/audio_dump/start_audio_recording', StartAudioRecording)
        self.stop_audio_recording_srv = rospy.ServiceProxy('/audio_dump/stop_audio_recording', StopAudioRecording)
        self.wrist_pitch_velocity_srv = rospy.ServiceProxy('/wrist_pitch_controller/set_velocity', SetVelocity)
        
        self.wrist_pitch_command_pub = rospy.Publisher('wrist_pitch_controller/command', Float64)
        self.action_server = SimpleActionServer(ACTION_NAME,
                                                ShakePitchObjectAction,
                                                execute_cb=self.shake_pitch_object,
                                                auto_start=False)

    def initialize(self):
        rospy.loginfo('%s: waiting for wubble_grasp_status service' % ACTION_NAME)
        rospy.wait_for_service('/wubble_grasp_status')
        rospy.loginfo('%s: connected to wubble_grasp_status service' % ACTION_NAME)
        
        rospy.loginfo('%s: waiting for 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
#.........这里部分代码省略.........

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

#.........这里部分代码省略.........
        graspquat,
        start_angles,
        pos_spacing=0.03,
        rot_spacing=0.1,
        consistent_angle=math.pi / 7.0,
        collision_aware=True,
        collision_check_resolution=1,
        steps_before_abort=1,
        num_steps=0,
        ordered_collision_operations=None,
        frame="base_link",
    ):

        start_pose = self.create_pose_stamped(approachpos + approachquat, frame)
        goal_pose = self.create_pose_stamped(grasppos + graspquat, frame)

        return self.get_interpolated_ik_motion_plan(
            start_pose,
            goal_pose,
            start_angles,
            ARM_JOINTS,
            pos_spacing,
            rot_spacing,
            consistent_angle,
            collision_aware,
            collision_check_resolution,
            steps_before_abort,
            num_steps,
            ordered_collision_operations,
            frame,
        )

    def push_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

        current_state = rospy.wait_for_message("l_arm_controller/state", FollowJointTrajectoryFeedback)
        start_angles = current_state.actual.positions

        full_state = rospy.wait_for_message("/joint_states", JointState)

        req = GetPositionFKRequest()
        req.header.frame_id = "base_link"
        req.fk_link_names = [GRIPPER_LINK_FRAME]
        req.robot_state.joint_state = full_state

        if not self.set_planning_scene_diff_client():
            rospy.logerr("%s: failed to set planning scene diff" % ACTION_NAME)
            self.action_server.set_aborted()
            return

        pose = self.get_fk_srv(req).pose_stamped[0].pose

        frame_id = "base_link"

        approachpos = [pose.position.x, pose.position.y, pose.position.z]
        approachquat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]

        push_distance = 0.40
        grasppos = [pose.position.x, pose.position.y - push_distance, pose.position.z]
        graspquat = [0.00000, 0.00000, 0.70711, -0.70711]

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

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

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

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

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

    def drop_object(self, goal):
        if self.action_server.is_preempt_requested():
            rospy.loginfo('%s: preempted' % ACTION_NAME)
            self.action_server.set_preempted()
            
        # check that we have something in hand before dropping it
        grasp_status = self.get_grasp_status_srv()
        
        # if the object is still in hand after lift is done we are good
        if not grasp_status.is_hand_occupied:
            rospy.logerr('%s: gripper empty, nothing to drop' % ACTION_NAME)
            self.action_server.set_aborted()
            return
            
        # record sound padded by 0.5 seconds from start and 1.5 seconds from the end
        self.start_audio_recording_srv(InfomaxAction.DROP, goal.category_id)
        rospy.sleep(0.5)
        self.open_gripper()
        rospy.sleep(1.5)
        
        # check if gripper actually opened first
        sound_msg = None
        grasp_status = self.get_grasp_status_srv()
        
        # if there something in the gripper - drop failed
        if grasp_status.is_hand_occupied:
            self.stop_audio_recording_srv(False)
        else:
            sound_msg = self.stop_audio_recording_srv(True)
            
        # delete the object that we just dropped, we don't really know where it will land
        obj = AttachedCollisionObject()
        obj.object.header.stamp = rospy.Time.now()
        obj.object.header.frame_id = GRIPPER_LINK_FRAME
        obj.object.operation.operation = CollisionObjectOperation.REMOVE
        obj.object.id = goal.collision_object_name
        obj.link_name = GRIPPER_LINK_FRAME
        obj.touch_links = GRIPPER_LINKS
        
        self.attached_object_pub.publish(obj)
        
        if sound_msg:
            self.action_server.set_succeeded(DropObjectResult(sound_msg.recorded_sound))
        else:
            self.action_server.set_aborted()

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