Bump to v8

catkin_ws/src/gbs/CMakeLists.txt

@@ -67,6 +67,7 @@ add_service_files(
   GBSLockTool.srv
   GBSMeasureMove.srv
   GBSNailLinMove.srv
+  GBSNailLinMoveArray.srv
   GBSPTPMove.srv
   GBSReadCurJointPose.srv
   GBSReadSensor.srv
@@ -82,6 +83,9 @@ add_service_files(
   GBSNailerDown.srv
   GBSNailerUp.srv
   GBSReadFlange.srv
+  GBSBlowPanel.srv
+  GBSRunDoubleSpindle.srv
+  GBSStopDoubleSpindle.srv
 )
 
 ## Generate actions in the 'action' folder

catkin_ws/src/gbs/scripts/test_client_synch_rs_v8.py

+#!/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 naillinmovearray(cmd, id, bstop, s, ac, *pose):
+    req = GBSNailLinMoveArrayRequest()
+
+    req.id = id
+    req.bstop = bstop
+    req.s = s
+    req.ac = ac
+
+    for i in range(len(pose)/6):
+        j = 6*i
+        req.p.append(GBSPose())
+        req.p[i].pos.x = pose[j]
+        req.p[i].pos.y = pose[j+1]
+        req.p[i].pos.z = pose[j+2]
+        req.p[i].ori.c = pose[j+3]
+        req.p[i].ori.b = pose[j+4]
+        req.p[i].ori.a = pose[j+5]
+
+    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 rundoublespindle(cmd, rpm):
+    req = GBSRunDoubleSpindleRequest()
+    req.rpm = rpm
+    resp = cmd(req)
+    return resp
+
+def stopdoublespindle(cmd):
+    req = GBSStopDoubleSpindleRequest()
+    resp = cmd(req)
+    return resp
+
+def blowpanel(cmd):
+    req = GBSBlowPanelRequest()
+    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_naillinmovearray')
+    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')
+    rospy.wait_for_service('gbs_blowpanel')
+    rospy.wait_for_service('gbs_rundoublespindle')
+    rospy.wait_for_service('gbs_stopdoublespindle')
+    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_naillinmovearray = rospy.ServiceProxy('gbs_naillinmovearray', GBSNailLinMoveArray)
+        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)
+        gbs_blowpanel = rospy.ServiceProxy('gbs_blowpanel', GBSBlowPanel)
+        gbs_rundoublespindle = rospy.ServiceProxy('gbs_rundoublespindle', GBSRunDoubleSpindle)
+        gbs_stopdoublespindle = rospy.ServiceProxy('gbs_stopdoublespindle', GBSStopDoubleSpindle)
+
+
+
+        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))
+
+        print "Nail move array"
+        ans = naillinmovearray(gbs_naillinmovearray, 2, 1, 1, 1, 0, 0, -0.6, math.radians(0), math.radians(0), math.radians(-175), 1, 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)
+
+        print "Blow panel"
+        ans = blowpanel(gbs_blowpanel)
+
+        print "Run double spindle"
+        ans = rundoublespindle(gbs_rundoublespindle, 5000)
+
+        print "Stop double spindle"
+        ans = stopdoublespindle(gbs_stopdoublespindle)
+
+    except rospy.ServiceException, e:
+        print "Service call failed: %s"%e
+
+def usage():
+    return "%s".s.argv[0]
+
+if __name__ == "__main__":
+    test()
+

catkin_ws/src/gbs/srv/GBSBlowPanel.srv

+---
+float32 err

catkin_ws/src/gbs/srv/GBSNailLinMoveArray.srv

+int32 id
+bool bstop
+float32 s
+float32 ac
+GBSPose[] p
+---
+float32 err1
+float32 err2

catkin_ws/src/gbs/srv/GBSRunDoubleSpindle.srv

+float32 rpm
+---
+float32 err

catkin_ws/src/gbs/srv/GBSStopDoubleSpindle.srv

+---
+float32 err