Add synchronous ros services

catkin_ws/src/gbs/CMakeLists.txt

@@ -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/GBSAcc.msg

+float32 ac

catkin_ws/src/gbs/msg/GBSAns.msg

+float32 err

catkin_ws/src/gbs/msg/GBSBStop.msg

+float32 bstop

catkin_ws/src/gbs/msg/GBSEuler.msg

+float32 c
+float32 b
+float32 a

catkin_ws/src/gbs/msg/GBSId.msg

+float32 id

catkin_ws/src/gbs/msg/GBSJointPose.msg

+float32 j1
+float32 j2
+float32 j3
+float32 j4
+float32 j5
+float32 j6

catkin_ws/src/gbs/msg/GBSPos.msg

+float32 x
+float32 y
+float32 z

catkin_ws/src/gbs/msg/GBSPose.msg

+GBSPos pos
+GBSEuler ori

catkin_ws/src/gbs/msg/GBSRPM.msg

+float32 rpm

catkin_ws/src/gbs/msg/GBSSensor.msg

+float32 x1
+float32 x2
+float32 y1

catkin_ws/src/gbs/msg/GBSSpeed.msg

+float32 s

catkin_ws/src/gbs/scripts/test_client_synch.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 = 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
+    req.pf.ori.b = bf
+    req.pf.ori.a = af
+    req.s.s = s
+    req.ac.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 = 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"%sys.argv[0]
+
+if __name__ == "__main__":
+    test()
+

catkin_ws/src/gbs/srv/GBSCircMove.srv

+GBSPose pi
+GBSPose pf
+GBSSpeed s
+GBSAcc ac
+---
+GBSAns ans1
+GBSAns ans2

catkin_ws/src/gbs/srv/GBSCombMove.srv

+int32 n
+float32[1000] array
+---
+GBSAns ans1
+GBSAns ans2

catkin_ws/src/gbs/srv/GBSLinMove.srv

+GBSPose p
+GBSSpeed s
+GBSAcc ac
+---
+GBSAns ans1
+GBSAns ans2

catkin_ws/src/gbs/srv/GBSLockSpindle.srv

+---
+GBSAns ans

catkin_ws/src/gbs/srv/GBSLockTool.srv

+---
+GBSAns ans

catkin_ws/src/gbs/srv/GBSMeasureMove.srv

+GBSId id
+GBSPose p
+GBSSpeed s
+GBSAcc ac
+---
+GBSAns ans1
+GBSAns ans2

catkin_ws/src/gbs/srv/GBSNailLinMove.srv

+GBSId id
+GBSBStop bstop
+GBSSpeed s
+GBSAcc ac
+GBSPose p1
+GBSPose p2
+---
+GBSAns ans1
+GBSAns ans2

catkin_ws/src/gbs/srv/GBSPTPMove.srv

+GBSJointPose p
+GBSSpeed s
+GBSAcc ac
+---
+GBSAns ans1
+GBSAns ans2

catkin_ws/src/gbs/srv/GBSReadCurJointPose.srv

+---
+GBSAns ans
+GBSJointPose p

catkin_ws/src/gbs/srv/GBSReadSensor.srv

+---
+GBSAns ans
+GBSSensor data

catkin_ws/src/gbs/srv/GBSReleasePanel.srv

+---
+GBSAns ans

catkin_ws/src/gbs/srv/GBSRunSpindle.srv

+GBSRPM rpm
+---
+GBSAns ans

catkin_ws/src/gbs/srv/GBSShakeVacuum.srv

+---
+GBSAns ans

catkin_ws/src/gbs/srv/GBSStopSpindle.srv

+---
+GBSAns ans

catkin_ws/src/gbs/srv/GBSSwitchVacuum.srv

+---
+GBSAns ans

catkin_ws/src/gbs/srv/GBSTakePanel.srv

+---
+GBSAns ans

catkin_ws/src/gbs/srv/GBSUnlockSpindle.srv

+---
+GBSAns ans

catkin_ws/src/gbs/srv/GBSUnlockTool.srv

+---
+GBSAns ans