grader.py 文件源码

def goto_pose(self, name, T, timeout):
        self.server.setPose("move_arm_marker", convert_to_message(T))
        self.server.applyChanges()
        self.pub_command.publish(convert_to_trans_message(T))
        print 'Goal published'        
        start_time = time.time()
        done = False
        while not done and not rospy.is_shutdown():

            self.mutex.acquire()
            last_joint_state = deepcopy(self.joint_state)
            self.mutex.release()
            if not self.check_validity(last_joint_state):
                print 'COLLISION!'

            try:
                (trans,rot) = self.listener.lookupTransform('/world_link','/lwr_arm_7_link',
                                                            rospy.Time(0))
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                print "TF Exception!"
                continue

            TR = numpy.dot(tf.transformations.translation_matrix(trans), 
                           tf.transformations.quaternion_matrix(rot))

            if (is_same(T, TR)): 
                print name + ": Reached goal"
                done = True

            if (time.time() - start_time > timeout) :
                done = True
                print name + ": Robot took too long to reach goal. Grader timed out"
            else:
                rospy.sleep(0.05)