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()
This diff is collapsed.
 #!/usr/bin/env python import sys import rospy from gbs.msg import * from gbs.srv import * def linmove(array, x, y, z, c, b, a, s, ac): i = array.n array.array[0+i] = 10 array.array[1+i] = 1 array.array[2+i] = 1 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.n = i + 11 def ptpmove(array, j1, j2, j3, j4, j5, j6, s, ac): i = array.n array.array[0+i] = 10 array.array[1+i] = 1 array.array[2+i] = 2 array.array[3+i] = j1 array.array[4+i] = j2 array.array[5+i] = j3 array.array[6+i] = j4 array.array[7+i] = j5 array.array[8+i] = j6 array.array[9+i] = s array.array[10+i] = ac array.n = i + 11 def readlibrary(array): i = array.n array.array[0+i] = 2 array.array[1+i] = 1 array.array[2+i] = 4 array.n = i + 3 def readcurrentrobotpos(array): i = array.n array.array[0+i] = 2 array.array[1+i] = 1 array.array[2+i] = 5 array.n = i + 3 def locktool(array): i = array.n array.array[0+i] = 2 array.array[1+i] = 1 array.array[2+i] = 6 array.n = i + 3 def unlocktool(array): i = array.n array.array[0+i] = 2 array.array[1+i] = 1 array.array[2+i] = 7 array.n = i + 3 def naillinmove(array, id, bstop, s, ac, x1, y1, z1, c1, b1, a1, x2, y2, z2, c2, b2, a2): i = array.n array.array[0+i] = 18 array.array[1+i] = 1 array.array[2+i] = 8 array.array[3+i] = id array.array[4+i] = bstop array.array[5+i] = s array.array[6+i] = ac array.array[7+i] = x1 array.array[8+i] = y1 array.array[9+i] = z1 array.array[10+i] = c1 array.array[11+i] = b1 array.array[12+i] = a1 array.array[13+i] = x2 array.array[14+i] = y2 array.array[15+i] = z2 array.array[16+i] = c2 array.array[17+i] = b2 array.array[18+i] = a2 array.n = i + 19 def measuremove(array, id, x, y, z, c, b, a, s, ac): i = array.n array.array[0+i] = 11 array.array[1+i] = 1 array.array[2+i] = 8 array.array[3+i] = id array.array[4+i] = x array.array[5+i] = y array.array[6+i] = z array.array[7+i] = c array.array[8+i] = b array.array[9+i] = a array.array[10+i] = s array.array[11+i] = ac array.n = i + 12 def circmove(array, xi, yi, zi, ci, bi, ai, xf, yf, zf, cf, bf, af, s, ac): i = array.n array.array[0+i] = 16 array.array[1+i] = 1 array.array[2+i] = 10 array.array[3+i] = xi array.array[4+i] = yi array.array[5+i] = zi array.array[6+i] = ci array.array[7+i] = bi array.array[8+i] = ai array.array[9+i] = xf array.array[10+i] = yf array.array[11+i] = zf array.array[12+i] = cf array.array[13+i] = bf array.array[14+i] = af array.array[15+i] = s array.array[16+i] = ac array.n = i + 17 def combmove(array, cmdarray): i = array.n array.array[0+i] = 35 array.array[1+i] = 1 array.array[2+i] = 11 for j in range(len(cmdarray)): array.array[3+i+j] = cmdarray[j] array.n = i + 3 + len(cmdarray) def runspindle(array, rpm): i = array.n array.array[0+i] = 3 array.array[1+i] = 1 array.array[2+i] = 12 array.array[3+i] = rpm array.n = i + 4 def stopspindle(array): i = array.n array.array[0+i] = 2 array.array[1+i] = 1 array.array[2+i] = 13 array.n = i + 3 def lockspindle(array): i = array.n array.array[0+i] = 2 array.array[1+i] = 1 array.array[2+i] = 14 array.n = i + 3 def unlockspindle(array): i = array.n array.array[0+i] = 2 array.array[1+i] = 1 array.array[2+i] = 15 array.n = i + 3 def getsensorvalues(array): i = array.n array.array[0+i] = 2 array.array[1+i] = 1 array.array[2+i] = 16 array.n = i + 3 def takepanel(array): i = array.n array.array[0+i] = 2 array.array[1+i] = 1 array.array[2+i] = 17 array.n = i + 3 def releasepanel(array): i = array.n array.array[0+i] = 2 array.array[1+i] = 1 array.array[2+i] = 18 array.n = i + 3 def switchvacuum(array): i = array.n array.array[0+i] = 2 array.array[1+i] = 1 array.array[2+i] = 19 array.n = i + 3 def shakevacuum(array): i = array.n array.array[0+i] = 2 array.array[1+i] = 1 array.array[2+i] = 20 array.n = i + 3 def post_gbs_cmdarray_client(): rospy.wait_for_service('post_gbs_cmdarray') try: post_gbs_cmdarray = rospy.ServiceProxy('post_gbs_cmdarray', PostGBSCmdArray) array = GBSCmdArray() array.n = 0 linmove(array, 10, 11, 12, 35, 0, 0, 10, 5) linmove(array, 10, 11, 12, 35, 0, 0, 10, 6) ptpmove(array, 1, 2, 3, 4, 5, 6, 10, 6) readlibrary(array) readcurrentrobotpos(array) locktool(array) unlocktool(array) naillinmove(array, 1, 1, 3, 3, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12) measuremove(array, 2, 1, 2, 3, 4, 5, 6, 10, 5) circmove(array, 0, 0, 0, 0, 0, 0, 100, 0, 0, 0, 0, 0, 10, 5) combmove(array, (1, 0,0,0,0,0,0,10,5, 10, 100,0,0,35,0,0,15,25,35,1,2,3,10,5, 2, 1,2,3,4,5,6,10,5)) runspindle(array, 5000) stopspindle(array) lockspindle(array) unlockspindle(array) getsensorvalues(array) takepanel(array) releasepanel(array) switchvacuum(array) shakevacuum(array) array.array[array.n] = -1 resp = post_gbs_cmdarray(array) except rospy.ServiceException, e: print "Service call failed: %s"%e def usage(): return "%s"%sys.argv[0] if __name__ == "__main__": post_gbs_cmdarray_client()