Commit a2a33023 by Mathias Haage

### Add scripts for version 3

parent 899f7672
 #!/usr/bin/env python from gbs.srv import * import rospy class Array: n = 0 array = [0 for i in range(100)] 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_gbs_circmove(req): array = Array() array.n = 17 array.array = [16,1,10, req.pi.pos.x,req.pi.pos.y,req.pi.pos.z, req.pi.ori.c,req.pi.ori.b,req.pi.ori.a, req.pf.pos.x,req.pf.pos.y,req.pf.pos.z, req.pf.ori.c,req.pf.ori.b,req.pf.ori.a, req.s, req.ac] prettyprint(array) ans = GBSCircMoveResponse() ans.err1 = 0 ans.err2 = 0 return ans def handle_gbs_combmove(req): array = Array() array.n = 3 + req.n array.array = [35,1,11] + [-1 for i in range(req.n + 1)] for i in range(req.n): array.array[3 + i] = req.array[i] prettyprint(array) ans = GBSCombMoveResponse() ans.err1 = 0 ans.err2 = 0 return ans def handle_gbs_linmove(req): array = Array() array.n = 11 array.array = [10,1,1, req.p.pos.x,req.p.pos.y,req.p.pos.z, req.p.ori.c,req.p.ori.b,req.p.ori.a, req.s, req.ac] prettyprint(array) ans = GBSLinMoveResponse() ans.err1 = 0 ans.err2 = 0 return ans def handle_gbs_lockspindle(req): array = Array() array.n = 3 array.array = [2,1,14] prettyprint(array) ans = GBSLockSpindleResponse() ans.err = 0 return ans def handle_gbs_locktool(req): array = Array() array.n = 3 array.array = [2,1,6] prettyprint(array) ans = GBSLockToolResponse() ans.err = 0 return ans def handle_gbs_measuremove(req): array = Array() array.n = 12 array.array = [11,1,8, req.id, req.p.pos.x,req.p.pos.y,req.p.pos.z, req.p.ori.c,req.p.ori.b,req.p.ori.a, req.s, req.ac] prettyprint(array) ans = GBSMeasureMoveResponse() ans.err1 = 0 ans.err2 = 0 return ans def handle_gbs_naillinmove(req): array = Array() array.n = 19 array.array = [18,1,8, req.id, req.bstop, req.s, req.ac, req.p1.pos.x,req.p1.pos.y,req.p1.pos.z, req.p1.ori.c,req.p1.ori.b,req.p1.ori.a, req.p2.pos.x,req.p2.pos.y,req.p2.pos.z, req.p2.ori.c,req.p2.ori.b,req.p2.ori.a] prettyprint(array) ans = GBSNailLinMoveResponse() ans.err1 = 0 ans.err2 = 0 return ans def handle_gbs_ptpmove(req): array = Array() array.n = 11 array.array = [10,1,2, req.p.j1, req.p.j2, req.p.j3, req.p.j4, req.p.j5, req.p.j6, req.s, req.ac] prettyprint(array) ans = GBSPTPMoveResponse() ans.err1 = 0 ans.err2 = 0 return ans def handle_gbs_readcurjointpose(req): array = Array() array.n = 3 array.array = [2,1,5] prettyprint(array) ans = GBSReadCurJointPoseResponse() ans.err = 0 ans.p.j1 = 0 # ... return ans def handle_gbs_releasepanel(req): array = Array() array.n = 3 array.array = [2,1,18] prettyprint(array) ans = GBSReleasePanelResponse() ans.err = 0 return ans def handle_gbs_runspindle(req): array = Array() array.n = 4 array.array = [3,1,12, req.rpm] prettyprint(array) ans = GBSRunSpindleResponse() return ans def handle_gbs_readsensor(req): array = Array() array.n = 3 array.array = [2,1,16] prettyprint(array) ans = GBSReadSensorResponse() ans.err = 0 ans.data.x1 = 0 # ... return ans def handle_gbs_shakevacuum(req): array = Array() array.n = 3 array.array = [2,1,20] prettyprint(array) ans = GBSShakeVacuumResponse() ans.err = 0 return ans def handle_gbs_stopspindle(req): array = Array() array.n = 3 array.array = [2,1,13] prettyprint(array) ans = GBSStopSpindleResponse() ans.err = 0 return ans def handle_gbs_switchvacuum(req): array = Array() array.n = 3 array.array = [2,1,19] prettyprint(array) ans = GBSSwitchVacuumResponse() ans.err = 0 return ans def handle_gbs_takepanel(req): array = Array() array.n = 3 array.array = [2,1,17] prettyprint(array) ans = GBSTakePanelResponse() ans.err = 0 return ans def handle_gbs_unlockspindle(req): array = Array() array.n = 3 array.array = [2,1,15] prettyprint(array) ans = GBSUnlockSpindleResponse() ans.err = 0 return ans def handle_gbs_unlocktool(req): array = Array() array.n = 3 array.array = [2,1,7] prettyprint(array) ans = GBSUnlockToolResponse() ans.err = 0 return ans def add_services(): rospy.init_node('synch_gbs_server') rospy.Service('gbs_circmove', GBSCircMove, handle_gbs_circmove) rospy.Service('gbs_combmove', GBSCombMove, handle_gbs_combmove) rospy.Service('gbs_linmove', GBSLinMove, handle_gbs_linmove) rospy.Service('gbs_lockspindle', GBSLockSpindle, handle_gbs_lockspindle) rospy.Service('gbs_locktool', GBSLockTool, handle_gbs_locktool) rospy.Service('gbs_measuremove', GBSMeasureMove, handle_gbs_measuremove) rospy.Service('gbs_naillinmove', GBSNailLinMove, handle_gbs_naillinmove) rospy.Service('gbs_ptpmove', GBSPTPMove, handle_gbs_ptpmove) rospy.Service('gbs_jointpose', GBSReadCurJointPose, handle_gbs_readcurjointpose) rospy.Service('gbs_releasepanel', GBSReleasePanel, handle_gbs_releasepanel) rospy.Service('gbs_runspindle', GBSRunSpindle, handle_gbs_runspindle) rospy.Service('gbs_readsensor', GBSReadSensor, handle_gbs_readsensor) rospy.Service('gbs_shakevacuum', GBSShakeVacuum, handle_gbs_shakevacuum) rospy.Service('gbs_stopspindle', GBSStopSpindle, handle_gbs_stopspindle) rospy.Service('gbs_switchvacuum', GBSSwitchVacuum, handle_gbs_switchvacuum) rospy.Service('gbs_takepanel', GBSTakePanel, handle_gbs_takepanel) rospy.Service('gbs_unlockspindle', GBSUnlockSpindle, handle_gbs_unlockspindle) rospy.Service('gbs_unlocktool', GBSUnlockTool, handle_gbs_unlocktool) print "Ready to receive GBS commands" rospy.spin() if __name__ == "__main__": add_services()
 #!/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 req.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 req.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 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 req.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 req.bstop = bstop req.s = s req.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 req.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 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')