Commit a2a33023 authored by Mathias Haage's avatar Mathias Haage

Add scripts for version 3

parent 899f7672
This diff is collapsed.
#!/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')
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)
ans = shakevacuum(gbs_shakevacuum)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def usage():
return "%s".s.argv[0]
if __name__ == "__main__":
test()
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment