Commit e795e860 authored by Mathias Haage's avatar Mathias Haage

Update to joint measure move

parent fdc16d64
This diff is collapsed.
#!/usr/bin/env python
import sys
import math
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, j1, j2, j3, j4, j5, j6, s, ac):
req = GBSMeasureMoveRequest()
req.id = id
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 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 getmeasure(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 readlaser(cmd):
req = GBSReadLaserRequest()
resp = cmd(req)
return resp
def nailerdown(cmd):
req = GBSNailerDownRequest()
req.id = 1
resp = cmd(req)
return resp
def nailerup(cmd):
req = GBSNailerUpRequest()
req.id = 1
resp = cmd(req)
return resp
def readflange(cmd):
req = GBSReadFlangeRequest()
resp = cmd(req)
return resp
def test_readflange(gbs_readflange):
ans = readflange(gbs_readflange)
print "readflange:"
print "x " + str(ans.pose.pos.x)
print "y " + str(ans.pose.pos.y)
print "z " + str(ans.pose.pos.z)
print "c " + str(ans.pose.ori.c)
print "b " + str(ans.pose.ori.b)
print "a " + str(ans.pose.ori.a)
print
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')
rospy.wait_for_service('gbs_readlaser')
rospy.wait_for_service('gbs_nailerdown')
rospy.wait_for_service('gbs_nailerup')
rospy.wait_for_service('gbs_readflange')
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)
gbs_readlaser = rospy.ServiceProxy('gbs_readlaser', GBSReadLaser)
gbs_nailerdown = rospy.ServiceProxy('gbs_nailerdown', GBSNailerDown)
gbs_nailerup = rospy.ServiceProxy('gbs_nailerup', GBSNailerUp)
gbs_readflange = rospy.ServiceProxy('gbs_readflange', GBSReadFlange)
print "Lin Move"
ans = linmove(gbs_linmove, 2, 0, -0.6, math.radians(0), math.radians(0), math.radians(-175), 1, 1)
test_readflange(gbs_readflange)
print "Lin Move"
ans = linmove(gbs_linmove, 0, 0, -0.6, math.radians(0), math.radians(0), math.radians(-175), 1, 1)
test_readflange(gbs_readflange)
print "PTP Move"
ans = ptpmove(gbs_ptpmove, 2000, 0, 0, 0, 5, 0, 1, 1)
print "PTP Move"
ans = ptpmove(gbs_ptpmove, 1000, 0, 0, 0, 5, 0, 1, 1)
##readlibrary(array)
print "Read joint pose"
ans = readcurrentrobotpos(gbs_jointpose)
print 'Current robot pose ['+str(ans.p.j1)+','+str(ans.p.j2)+','+str(ans.p.j3)+','+str(ans.p.j4)+','+str(ans.p.j5)+','+str(ans.p.j6)+']'
print "Lock tool"
ans = locktool(gbs_locktool)
print "Unlock tool"
ans = unlocktool(gbs_unlocktool)
print "Measure move"
ans = measuremove(gbs_measuremove, 1000, 0, 0, 0, 5, 0, 0, 1, 1)
print "Nail move"
ans = naillinmove(gbs_naillinmove, 1, 1, 1, 1, 0, 0, -0.6, math.radians(0), math.radians(0), math.radians(-175), 2, 0, -0.6, math.radians(0), math.radians(0), math.radians(-175))
##ans = circmove(gbs_circmove, 0, 0, 0, 0, 0, 0, 100, 0, 0, 0, 0, 0, 10, 5)
#print "Move"
#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))
print "Run spindle"
ans = runspindle(gbs_runspindle, 5000)
print "Stop spindle"
ans = stopspindle(gbs_stopspindle)
print "Lock spindle"
ans = lockspindle(gbs_lockspindle)
print "Unlock spindle"
ans = unlockspindle(gbs_unlockspindle)
print "Get measure"
ans = getmeasure(gbs_readsensor)
print "Take panel"
ans = takepanel(gbs_takepanel)
print "Release panel"
ans = releasepanel(gbs_releasepanel)
print "Switch vacuum"
ans = switchvacuum(gbs_switchvacuum)
print "Shake vacuum"
ans = shakevacuum(gbs_shakevacuum)
print "Nailer down"
ans = nailerdown(gbs_nailerdown)
print "Nailer up"
ans = nailerup(gbs_nailerup)
print "Read laser"
ans = readlaser(gbs_readlaser)
print "Distance "+str(ans.distance)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def usage():
return "%s".s.argv[0]
if __name__ == "__main__":
test()
int32 id
GBSPose p
GBSJointPose p
float32 s
float32 ac
---
......
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