Add scripts for version 3

catkin_ws/src/gbs/scripts/synch_gbs_server_v3.py

+#!/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()
+

catkin_ws/src/gbs/scripts/test_client_synch_v3.py

+#!/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')