Skip to content

GitLab

Commit d9c7958b authored by Mathias Haage's avatar Mathias Haage
Browse files

Add synchronous ros services

parent 3e95cb36
Hide whitespace changes
Inline Side-by-side
catkin_ws/src/gbs/CMakeLists.txt
View file @ d9c7958b
......@@ -47,13 +47,42 @@ find_package(catkin REQUIRED COMPONENTS
## Generate messages in the 'msg' folder
add_message_files(
FILES
GBSCmdArray.msg
GBSAcc.msg
GBSAckArray.msg
GBSAns.msg
GBSBStop.msg
GBSCmdArray.msg
GBSEuler.msg
GBSId.msg
GBSJointPose.msg
GBSPose.msg
GBSPos.msg
GBSRPM.msg
GBSSensor.msg
GBSSpeed.msg
)
## Generate services in the 'srv' folder
add_service_files(
FILES
GBSCircMove.srv
GBSCombMove.srv
GBSLinMove.srv
GBSLockSpindle.srv
GBSLockTool.srv
GBSMeasureMove.srv
GBSNailLinMove.srv
GBSPTPMove.srv
GBSReadCurJointPose.srv
GBSReadSensor.srv
GBSReleasePanel.srv
GBSRunSpindle.srv
GBSShakeVacuum.srv
GBSStopSpindle.srv
GBSSwitchVacuum.srv
GBSTakePanel.srv
GBSUnlockSpindle.srv
GBSUnlockTool.srv
PostGBSCmdArray.srv
)
......
catkin_ws/src/gbs/msg/GBSEuler.msg 0 → 100644
View file @ d9c7958b
float32 c
float32 b
float32 a
catkin_ws/src/gbs/msg/GBSJointPose.msg 0 → 100644
View file @ d9c7958b
float32 j1
float32 j2
float32 j3
float32 j4
float32 j5
float32 j6
catkin_ws/src/gbs/msg/GBSPos.msg 0 → 100644
View file @ d9c7958b
float32 x
float32 y
float32 z
catkin_ws/src/gbs/msg/GBSSensor.msg 0 → 100644
View file @ d9c7958b
float32 x1
float32 x2
float32 y1
catkin_ws/src/gbs/scripts/synch_gbs_server.py 0 → 100755
View file @ d9c7958b
#!/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.s, req.ac.ac]
prettyprint(array)
ans = GBSCircMoveResponse()
ans.ans1.err = 0
ans.ans2.err = 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.ans1.err = 0
ans.ans2.err = 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.s, req.ac.ac]
prettyprint(array)
ans = GBSLinMoveResponse()
ans.ans1.err = 0
ans.ans2.err = 0
return ans
def handle_gbs_lockspindle(req):
array = Array()
array.n = 3
array.array = [2,1,14]
prettyprint(array)
ans = GBSLockSpindleResponse()
ans.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.ans.err = 0
return ans
def handle_gbs_measuremove(req):
array = Array()
array.n = 12
array.array = [11,1,8, req.id.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.s, req.ac.ac]
prettyprint(array)
ans = GBSMeasureMoveResponse()
ans.ans1.err = 0
ans.ans2.err = 0
return ans
def handle_gbs_naillinmove(req):
array = Array()
array.n = 19
array.array = [18,1,8, req.id.id, req.bstop.bstop, req.s.s, req.ac.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.ans1.err = 0
ans.ans2.err = 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.s, req.ac.ac]
prettyprint(array)
ans = GBSPTPMoveResponse()
ans.ans1.err = 0
ans.ans2.err = 0
return ans
def handle_gbs_readcurjointpose(req):
array = Array()
array.n = 3
array.array = [2,1,5]
prettyprint(array)
ans = GBSReadCurJointPoseResponse()
ans.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.ans.err = 0
return ans
def handle_gbs_runspindle(req):
array = Array()
array.n = 4
array.array = [3,1,12, req.rpm.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.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.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.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.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.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.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.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()
catkin_ws/src/gbs/scripts/test_client_synch.py 0 → 100755
View file @ d9c7958b
#!/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