Skip to content

G
GBS
Commit 899f7672 authored by Mathias Haage's avatar Mathias Haage
Browse files
Options

Remove scripts for previous versions

parent 60ccc941
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 0 additions and 1056 deletions
catkin_ws/src/gbs/scripts/post_gbs_cmdarray.py deleted 100755 → 0
View file @ 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()
catkin_ws/src/gbs/scripts/synch_gbs_server.py deleted 100755 → 0
View file @ 60ccc941
This diff is collapsed.
catkin_ws/src/gbs/scripts/test_client.py deleted 100755 → 0
View file @ 60ccc941
#!/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()
catkin_ws/src/gbs/scripts/test_client_synch.py deleted 100755 → 0
View file @ 60ccc941
#!/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)
gbs_unlocktool = rospy.ServiceProxy('gbs_unlocktool', GBSUnlockTool)
gbs_measuremove = rospy.ServiceProxy('gbs_measuremove', GBSMeasureMove)
gbs_naillinmove = rospy.ServiceProxy('gbs_naillinmove', GBSNailLinMove)
gbs_circmove = rospy.ServiceProxy('gbs_circmove', GBSCircMove)
gbs_combmove = rospy.ServiceProxy('gbs_combmove', GBSCombMove)
gbs_runspindle = rospy.ServiceProxy('gbs_runspindle', GBSRunSpindle)
gbs_stopspindle = rospy.ServiceProxy('gbs_stopspindle', GBSStopSpindle)
gbs_lockspindle = rospy.ServiceProxy('gbs_lockspindle', GBSLockSpindle)
gbs_unlockspindle = rospy.ServiceProxy('gbs_unlockspindle', GBSUnlockSpindle)
gbs_readsensor = rospy.ServiceProxy('gbs_readsensor', GBSReadSensor)
gbs_takepanel = rospy.ServiceProxy('gbs_takepanel', GBSTakePanel)
gbs_releasepanel = rospy.ServiceProxy('gbs_releasepanel', GBSReleasePanel)
gbs_switchvacuum = rospy.ServiceProxy('gbs_switchvacuum', GBSSwitchVacuum)
gbs_shakevacuum = rospy.ServiceProxy('gbs_shakevacuum', GBSShakeVacuum)
ans = linmove(gbs_linmove, 10, 11, 12, 35, 0, 0, 10, 5)
ans = linmove(gbs_linmove, 10, 11, 12, 35, 0, 0, 10, 6)
ans = ptpmove(gbs_ptpmove, 1, 2, 3, 4, 5, 6, 10, 6)
#readlibrary(array)
ans = readcurrentrobotpos(gbs_jointpose)
ans = locktool(gbs_locktool)
ans = unlocktool(gbs_unlocktool)
ans = measuremove(gbs_measuremove, 111, 10, 11, 12, 35, 0, 0, 10, 6)
ans = naillinmove(gbs_naillinmove, 1, 1, 3, 3, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12)
ans = circmove(gbs_circmove, 0, 0, 0, 0, 0, 0, 100, 0, 0, 0, 0, 0, 10, 5)
ans = combmove(gbs_combmove, (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))
ans = runspindle(gbs_runspindle, 5000)
ans = stopspindle(gbs_stopspindle)
ans = lockspindle(gbs_lockspindle)
ans = unlockspindle(gbs_unlockspindle)
ans = getsensorvalues(gbs_readsensor)
ans = takepanel(gbs_takepanel)
ans = releasepanel(gbs_releasepanel)
ans = switchvacuum(gbs_switchvacuum)