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()
 #!/usr/bin/env python import sys import rospy from gbs.msg import * from gbs.srv import * def linmove(cmd, x, y, z, c, b, a, s, ac): req = GBSLinMoveRequest() req.p.pos.x = x req.p.pos.y = y req.p.pos.z = z req.p.ori.c = c req.p.ori.b = b req.p.ori.a = a req.s.s = s req.ac.ac = ac resp = cmd(req) return resp def ptpmove(cmd, j1, j2, j3, j4, j5, j6, s, ac): req = GBSPTPMoveRequest() req.p.j1 = j1 req.p.j2 = j2 req.p.j3 = j3 req.p.j4 = j4 req.p.j5 = j5 req.p.j6 = j6 req.s.s = s req.ac.ac = ac resp = cmd(req) return resp #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(cmd): req = GBSReadCurJointPoseRequest() resp = cmd(req) return resp def locktool(cmd): req = GBSLockToolRequest() resp = cmd(req) return resp def unlocktool(cmd): req = GBSUnlockToolRequest() resp = cmd(req) return resp def measuremove(cmd, id, x, y, z, c, b, a, s, ac): req = GBSMeasureMoveRequest() req.id.id = id req.p.pos.x = x req.p.pos.y = y req.p.pos.z = z req.p.ori.c = c req.p.ori.b = b req.p.ori.a = a req.s.s = s req.ac.ac = ac resp = cmd(req) return resp def naillinmove(cmd, id, bstop, s, ac, x1, y1, z1, c1, b1, a1, x2, y2, z2, c2, b2, a2): req = GBSNailLinMoveRequest() req.id.id = id req.bstop.bstop = bstop req.s.s = s req.ac.ac = ac req.p1.pos.x = x1 req.p1.pos.y = y1 req.p1.pos.z = z1 req.p1.ori.c = c1 req.p1.ori.b = b1 req.p1.ori.a = a1 req.p2.pos.x = x2 req.p2.pos.y = y2 req.p2.pos.z = z2 req.p2.ori.c = c2 req.p2.ori.b = b2 req.p2.ori.a = a2 resp = cmd(req) return resp def circmove(cmd, xi, yi, zi, ci, bi, ai, xf, yf, zf, cf, bf, af, s, ac): req = GBSCircMoveRequest() req.pi.pos.x = xi req.pi.pos.y = yi req.pi.pos.z = zi req.pi.ori.c = ci req.pi.ori.b = bi req.pi.ori.a = ai req.pf.pos.x = xf req.pf.pos.y = yf req.pf.pos.z = zf req.pf.ori.c = cf req.pf.ori.b = bf req.pf.ori.a = af req.s.s = s req.ac.ac = ac resp = cmd(req) return resp def combmove(cmd, cmdarray): req = GBSCombMoveRequest() req.n = len(cmdarray) for j in range(len(cmdarray)): req.array[j] = cmdarray[j] resp = cmd(req) return resp def runspindle(cmd, rpm): req = GBSRunSpindleRequest() req.rpm.rpm = rpm resp = cmd(req) return resp def stopspindle(cmd): req = GBSStopSpindleRequest() resp = cmd(req) return resp def lockspindle(cmd): req = GBSLockSpindleRequest() resp = cmd(req) return resp def unlockspindle(cmd): req = GBSUnlockSpindleRequest() resp = cmd(req) return resp def getsensorvalues(cmd): req = GBSReadSensorRequest() resp = cmd(req) return resp def takepanel(cmd): req = GBSTakePanelRequest() resp = cmd(req) return resp def releasepanel(cmd): req = GBSReleasePanelRequest() resp = cmd(req) return resp def switchvacuum(cmd): req = GBSSwitchVacuumRequest() resp = cmd(req) return resp def shakevacuum(cmd): req = GBSShakeVacuumRequest() resp = cmd(req) return resp def test(): rospy.wait_for_service('gbs_linmove') rospy.wait_for_service('gbs_ptpmove') rospy.wait_for_service('gbs_jointpose') rospy.wait_for_service('gbs_locktool') rospy.wait_for_service('gbs_unlocktool') rospy.wait_for_service('gbs_measuremove') rospy.wait_for_service('gbs_naillinmove') rospy.wait_for_service('gbs_circmove') rospy.wait_for_service('gbs_combmove') rospy.wait_for_service('gbs_runspindle') rospy.wait_for_service('gbs_stopspindle') rospy.wait_for_service('gbs_lockspindle') rospy.wait_for_service('gbs_unlockspindle') rospy.wait_for_service('gbs_readsensor') rospy.wait_for_service('gbs_takepanel') rospy.wait_for_service('gbs_releasepanel') rospy.wait_for_service('gbs_switchvacuum') rospy.wait_for_service('gbs_shakevacuum') try: gbs_linmove = rospy.ServiceProxy('gbs_linmove', GBSLinMove) gbs_ptpmove = rospy.ServiceProxy('gbs_ptpmove', GBSPTPMove) gbs_jointpose = rospy.ServiceProxy('gbs_jointpose', GBSReadCurJointPose) gbs_locktool = rospy.ServiceProxy('gbs_locktool', GBSLockTool)