Add scripts for version 3

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')
+    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()
+