test_cam_transform.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import sys
00003 import roslib
00004 roslib.load_manifest('nao_description')
00005 import rospy
00006 import tf
00007 import numpy as np
00008 from tf import TransformListener
00009 from tf import transformations
00010 
00011 try:
00012     import motion
00013     from naoqi import ALProxy
00014 except ImportError:
00015     print("Error importing NaoQI. Please make sure that Aldebaran's NaoQI API is in your PYTHONPATH.")
00016     exit(1)
00017 
00018 
00019 # bla
00020 def space_to_str(space):
00021         if space == motion.SPACE_TORSO:
00022                 return "Torso"
00023         elif space == motion.SPACE_WORLD:
00024                 return "Odom"
00025         elif space == motion.SPACE_NAO:
00026                 return "BaseFootprint"
00027 
00028 def transform_to_str(result):
00029         tstr = ''
00030         maxColumnWidth = 20
00031         for i in range(0,4):
00032                 row = result[4*i:4*i+4]
00033                 rstr = ''.join(["%s" %str(k).center(maxColumnWidth) for k in (row)] )
00034                 rstr = rstr + '\n'
00035                 tstr = tstr+rstr
00036                 #tstr.append('\n')
00037         return tstr
00038 
00039 def np_mat_to_str(m):
00040     mm = list()
00041     for i in range(0,4):
00042         for j in range(0,4):
00043             mm.append(m[i,j])
00044 
00045     return transform_to_str(mm)
00046 
00047 def cam_rotation_matrix():
00048     return np.matrix('0 0 1 0;-1 0 0 0; 0 -1 0 0; 0 0 0 1')
00049 
00050 if __name__ == '__main__':
00051         rospy.init_node('test_tf')
00052         listener = TransformListener()
00053         ip = "ra.local"
00054         port = 9559
00055         proxy = ALProxy("ALMotion", ip, port)
00056         print "motionproxy ready"
00057         space = motion.SPACE_TORSO
00058         chainName = "Head"
00059         currentSensor = proxy.getPosition(chainName, space, True)
00060         current = proxy.getPosition(chainName, space, False)
00061         print "Position of %s in space %s:"%(chainName, space_to_str(space))
00062         print currentSensor
00063         print current
00064         rpy = currentSensor[3:]
00065         print rpy
00066         DCM = apply(transformations.euler_matrix,rpy)
00067         print DCM
00068         tTHSensor = proxy.getTransform(chainName, space, True)
00069         tTH = proxy.getTransform(chainName, space, False)
00070         print "Transform %s to %s:"%(space_to_str(space),chainName)
00071         print transform_to_str(tTHSensor)
00072         #print transform_to_str(tTH)
00073         chainName = "CameraTop"
00074         tTCSensor = proxy.getTransform(chainName, space, True)
00075         tTC = proxy.getTransform(chainName, space, False)
00076         print "Transform %s to %s:"%(space_to_str(space),chainName)
00077         print transform_to_str(tTCSensor)
00078         print
00079         T = np.matrix([tTCSensor[0:4],tTCSensor[4:8],tTCSensor[8:12],tTCSensor[12:]])
00080         #print cam_rotation_matrix()
00081         DCM = T*cam_rotation_matrix()
00082         print "Transform %s to %s (with rotated coordinate frame):"%(space_to_str(space),chainName)
00083         print np_mat_to_str(DCM)
00084 
00085         stamp = rospy.Time()
00086         frame1 = 'Torso_link'
00087         frame2 = 'CameraTop_frame'
00088         try:
00089             listener.waitForTransform(frame1,frame2,stamp,rospy.Duration(1.0))
00090             (trans,rot) = listener.lookupTransform(frame1,frame2,stamp)
00091         except tf.Exception as e:
00092             print "ERROR using TF"
00093             print "%s"%(e)
00094             sys.exit(-1)
00095 
00096         m = transformations.quaternion_matrix(rot)
00097         for i in range(0,3):
00098             m[i,3] = trans[i]
00099         print "[tf] Transform %s to %s:"%(frame1,frame2)
00100         print np_mat_to_str(m)
00101 
00102         e = np.linalg.norm(DCM - m)
00103         print "Error is: ",e
00104         if e > 1e-5:
00105             print "ERROR: Something is wrong with your TF transformations.  Transforms do not match!"
00106         else:
00107             print "Test ok. Done"
00108 
00109 


nao_description
Author(s): Armin Hornung, Stefan Osswald
autogenerated on Sat Jun 8 2019 20:38:41