Commit 899f7672 by Mathias Haage

Remove scripts for previous versions

parent 60ccc941
 #!/usr/bin/env python from gbs.srv import * import rospy def near(t, n): return n-0.01 < t and t < n+0.01 def prettyprint(array): i = 0 while i < array.n: c0 = array.array[0+i] c1 = array.array[1+i] c2 = array.array[2+i] if near(c0,10) and near(c1,1) and near(c2,1): x = array.array[3+i] y = array.array[4+i] z = array.array[5+i] c = array.array[6+i] b = array.array[7+i] a = array.array[8+i] s = array.array[9+i] ac = array.array[10+i] i = i + 11 print "linmove x=%f y=%f z=%f c=%f b=%f a=%f s=%f ac=%f"%(x,y,z,c,b,a,s,ac) elif near(c0,10) and near(c1,1) and near(c2,2): j1 = array.array[3+i] j2 = array.array[4+i] j3 = array.array[5+i] j4 = array.array[6+i] j5 = array.array[7+i] j6 = array.array[8+i] s = array.array[9+i] ac = array.array[10+i] i = i + 11 print "ptpmove j1=%f j2=%f j3=%f j4=%f j5=%f j6=%f s=%f ac=%f"%(j1,j2,j3,j4,j5,j6,s,ac) elif near(c0,2) and near(c1,1) and near(c2,4): i = i + 3 print "readlibrary" elif near(c0,2) and near(c1,1) and near(c2,5): i = i + 3 print "readcurrentrobotpos" elif near(c0,2) and near(c1,1) and near(c2,6): i = i + 3 print "locktool" elif near(c0,2) and near(c1,1) and near(c2,7): i = i + 3 print "unlocktool" elif near(c0,18) and near(c1,1) and near(c2,8): id = array.array[3+i] bstop = array.array[4+i] s = array.array[5+i] ac = array.array[6+i] x1 = array.array[7+i] y1 = array.array[8+i] z1 = array.array[9+i] c1 = array.array[10+i] b1 = array.array[11+i] a1 = array.array[12+i] x2 = array.array[13+i] y2 = array.array[14+i] z2 = array.array[15+i] c2 = array.array[16+i] b2 = array.array[17+i] a2 = array.array[18+i] i = i + 19 print "naillinmove id=%f bstop=%f s=%f ac=%f x1=%f y1=%f z1=%f c1=%f b1=%f a1=%f x2=%f y2=%f z2=%f c2=%f b2=%f a2=%f"%(id,bstop,s,ac,x1,y1,z1,c1,b1,a1,x2,y2,z2,c2,b2,a2) elif near(c0,11) and near(c1,1) and near(c2,8): id = array.array[3+i] x = array.array[4+i] y = array.array[5+i] z = array.array[6+i] c = array.array[7+i] b = array.array[8+i] a = array.array[9+i] s = array.array[10+i] ac = array.array[11+i] i = i + 12 print "measuremove id=%f x=%f y=%f z=%f c=%f b=%f a=%f s=%f ac=%f"%(id,x,y,z,c,b,a,s,ac) elif near(c0,16) and near(c1,1) and near(c2,10): xi = array.array[3+i] yi = array.array[4+i] zi = array.array[5+i] ci = array.array[6+i] bi = array.array[7+i] ai = array.array[8+i] xf = array.array[9+i] yf = array.array[10+i] zf = array.array[11+i] cf = array.array[12+i] bf = array.array[13+i] af = array.array[14+i] s = array.array[15+i] ac = array.array[16+i] i = i + 17 print "circmove xi=%f yi=%f zi=%f ci=%f bi=%f ai=%f xf=%f yf=%f zf=%f cf=%f bf=%f af=%f s=%f ac=%f"%(xi,yi,zi,ci,bi,ai,xf,yf,zf,cf,bf,af,s,ac) elif near(c0,35) and near(c1,1) and near(c2,11): i = i + 3 print "combmove" while True: cc0 = array.array[i] if near(cc0, 1): x = array.array[1+i] y = array.array[2+i] z = array.array[3+i] c = array.array[4+i] b = array.array[5+i] a = array.array[6+i] s = array.array[7+i] ac = array.array[8+i] i = i + 9 print " linmove x=%f y=%f z=%f c=%f b=%f a=%f s=%f ac=%f"%(x,y,z,c,b,a,s,ac) elif near(cc0, 10): xi = array.array[1+i] yi = array.array[2+i] zi = array.array[3+i] ci = array.array[4+i] bi = array.array[5+i] ai = array.array[6+i] xf = array.array[7+i] yf = array.array[8+i] zf = array.array[9+i] cf = array.array[10+i] bf = array.array[11+i] af = array.array[12+i] s = array.array[13+i] ac = array.array[14+i] i = i + 15 print " circmove xi=%f yi=%f zi=%f ci=%f bi=%f ai=%f xf=%f yf=%f zf=%f cf=%f bf=%f af=%f s=%f ac=%f"%(xi,yi,zi,ci,bi,ai,xf,yf,zf,cf,bf,af,s,ac) elif near(cc0, 2): j1 = array.array[1+i] j2 = array.array[2+i] j3 = array.array[3+i] j4 = array.array[4+i] j5 = array.array[5+i] j6 = array.array[6+i] s = array.array[7+i] ac = array.array[8+i] i = i + 9 print " ptpmove j1=%f j2=%f j3=%f j4=%f j5=%f j6=%f s=%f ac=%f"%(j1,j2,j3,j4,j5,j6,s,ac) else: break elif near(c0,3) and near(c1,1) and near(c2,12): rpm = array.array[3+i] i = i + 4 print "runspindle rpm=%f"%(rpm) elif near(c0,2) and near(c1,1) and near(c2,13): i = i + 3 print "stopspindle" elif near(c0,2) and near(c1,1) and near(c2,14): i = i + 3 print "lockspindle" elif near(c0,2) and near(c1,1) and near(c2,15): i = i + 3 print "unlockspindle" elif near(c0,2) and near(c1,1) and near(c2,16): i = i + 3 print "getsensorvalues" elif near(c0,2) and near(c1,1) and near(c2,17): i = i + 3 print "takepanel" elif near(c0,2) and near(c1,1) and near(c2,18): i = i + 3 print "releasepanel" elif near(c0,2) and near(c1,1) and near(c2,19): i = i + 3 print "switchvacuum" elif near(c0,2) and near(c1,1) and near(c2,20): i = i + 3 print "shakevacuum" else: print "Unknown GBS command %f %f %f"%(c0,c1,c2) return def handle_post_gbs_cmdarray(req): print "Received an GBS array" prettyprint(req.array) return PostGBSCmdArrayResponse() def add_post_gbs_cmdarray(): rospy.init_node('post_gbs_cmdarray_server') s = rospy.Service('post_gbs_cmdarray', PostGBSCmdArray, handle_post_gbs_cmdarray) print "Ready to receive GBS arrays" rospy.spin() if __name__ == "__main__": add_post_gbs_cmdarray()