grader.py 文件源码

def go_to_pose(self, name, T, timeout):
        msg = convert_to_trans_message(T)
        self.update_marker(T)
        start_time = time.time()
        done = False
        while not done and not rospy.is_shutdown():
            self.pub_cmd.publish(msg)
            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 desired pose"
                done = True

            if (time.time() - start_time > timeout) :
                print name + ": Robot took too long to reach desired pose"
                done = True
            else:
                rospy.sleep(0.1)