00001
00002
00003 import time
00004 from random import *
00005
00006 import roslib
00007 roslib.load_manifest('cob_script_server')
00008 import rospy
00009
00010 import tf
00011 from tf.transformations import euler_from_quaternion
00012
00013 import cv
00014 from sensor_msgs.msg import Image
00015 from cv_bridge import CvBridge, CvBridgeError
00016 from math import *
00017
00018
00019 from simple_script_server import script
00020
00021 class CalibCam(script):
00022
00023 def Initialize(self):
00024 self.bridge = CvBridge()
00025 self.image_sub = rospy.Subscriber("/stereo/right/image_color",Image,self.callback)
00026 self.cv_image = cv.CreateImage((1,1), 1 , 3)
00027 self.sss.init("torso")
00028
00029 def Run(self):
00030 print "start"
00031 seed()
00032 maxVal = 0.17
00033 file_path = "~/"
00034 listener = tf.TransformListener()
00035 nr_images = 14
00036
00037
00038 self.sss.move("arm","calib")
00039 self.sss.move("torso","home")
00040 self.sss.move("sdh","home")
00041
00042 self.sss.wait_for_input()
00043 self.sss.move("sdh","calib")
00044 self.sss.wait_for_input()
00045
00046
00047 for i in range(1,nr_images):
00048 if i==1:
00049 r1 = maxVal
00050 r2 = maxVal
00051 elif i==2:
00052 r1 = -maxVal
00053 r2 = maxVal
00054 elif i==3:
00055 r1 = maxVal
00056 r2 = -maxVal
00057 elif i==4:
00058 r1 = -maxVal
00059 r2 = -maxVal
00060 else:
00061 r1 = (random()-0.5)*2*maxVal;
00062 r2 = (random()-0.5)*2*maxVal;
00063 self.sss.move("torso",[[0.75*r1,0.75*r2,r1,r2]])
00064 self.sss.sleep(1)
00065 try:
00066 (trans,rot) = listener.lookupTransform('/base_link', '/head_color_camera_r_link', rospy.Time(0))
00067 rpy = euler_from_quaternion(rot)
00068 cyaw = cos(rpy[2])
00069 syaw = sin(rpy[2])
00070 cpitch = cos(rpy[1])
00071 spitch = sin(rpy[1])
00072 croll = cos(rpy[0])
00073 sroll = sin(rpy[0])
00074 R11 = cyaw*cpitch
00075 R12 = cyaw*spitch*sroll-syaw*croll
00076 R13 = cyaw*spitch*croll+syaw*sroll
00077 R21 = syaw*cpitch
00078 R22 = syaw*spitch*sroll+cyaw*croll
00079 R23 = syaw*spitch*croll-cyaw*sroll
00080 R31 = -spitch
00081 R32 = cpitch*sroll
00082 R33 = cpitch*croll
00083 fout = open(file_path+'calpic'+str(i)+'.coords','w')
00084 fout.write(str(R11)+' '+str(R12)+' '+str(R13)+' '+str(trans[0]*1000)+'\n'+str(R21)+' '+str(R22)+' '+str(R23)+' '+str(trans[1]*1000)+'\n'+str(R31)+' '+str(R32)+' '+str(R33)+' '+str(trans[2]*1000))
00085 fout.close()
00086 except (tf.LookupException, tf.ConnectivityException):
00087 print "tf exception"
00088
00089 self.sss.sleep(1)
00090 cv.SaveImage(file_path+'calpic'+str(i)+'.png',self.cv_image)
00091 self.sss.sleep(1)
00092 self.sss.move("torso","home")
00093 print "finished"
00094
00095 def callback(self,data):
00096 try:
00097 self.cv_image = self.bridge.imgmsg_to_cv(data, "bgr8")
00098 except CvBridgeError, e:
00099 print e
00100
00101 if __name__ == "__main__":
00102 SCRIPT = CalibCam()
00103 SCRIPT.Start()