Update to v5

catkin_ws/src/gbs/scripts/synch_gbs_server_rs_v3.py

@@ -5,7 +5,12 @@ import rospy
 import socket
 import struct
 import sys
+import argparse
 
+# HOST = '192.168.126.129' SIMULATION
+# HOST = '192.168.125.1' REAL ROBOT
+HOST = '192.168.126.129'
+PORT = 2500
 
 class Array:
     n = 0
@@ -13,13 +18,11 @@ class Array:
 
 
 class Comm:
-    HOST = 'vm100.cs.lth.se'
-    PORT = 2500
 
-    def connect(self):
+    def connect(self, host, port):
         self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
-        print "Connecting to %s %s"%(self.HOST, self.PORT)
-        self.s.connect((self.HOST, self.PORT))
+        print "Connecting to %s %s"%(host, port)
+        self.s.connect((host, port))
 
         self.packer = struct.Struct('!f')
 
@@ -40,6 +43,7 @@ class Comm:
     def sendArray(self, array):
         for i in range(array.n):
             self.sendFloat(array.array[i])
+        self.sendFloat(0.0)
 
     def receiveArray(self, n):
         array = Array()
@@ -452,7 +456,6 @@ def handle_gbs_unlocktool(req):
 
 
 def add_services():
-    comm.connect()
     rospy.init_node('synch_gbs_server')
     rospy.Service('gbs_circmove', GBSCircMove, handle_gbs_circmove)
     rospy.Service('gbs_combmove', GBSCombMove, handle_gbs_combmove)
@@ -476,5 +479,15 @@ def add_services():
     rospy.spin()
 
 if __name__ == "__main__":
+    #ip = ""
+    #port = 0
+
+    #parser = argparse.ArgumentParser(add_help=True)
+    #parser.add_argument('ip', help='IP address of the remote host')
+    #parser.add_argument('-p','--port', dest="port", type=int, default=2500, help='Port on the remote host')
+
+    #parsed=parser.parse_args(sys.argv[1:])
+
+    comm.connect(HOST, PORT)
     add_services()
 

catkin_ws/src/gbs/scripts/test_client_synch_rs_v4.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, 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 readlaser(cmd):
+    req = GBSReadLaserRequest()
+    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')
+    rospy.wait_for_service('gbs_readlaser')
+    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)
+        gbs_readlaser = rospy.ServiceProxy('gbs_readlaser', GBSReadLaser)
+
+        ans = linmove(gbs_linmove, 2, 0, -0.6, math.radians(0), math.radians(0), math.radians(-175), 1, 1)
+        ans = linmove(gbs_linmove, 0, 0, -0.6, math.radians(0), math.radians(0), math.radians(-175), 1, 1)
+        ans = ptpmove(gbs_ptpmove, 2000, 0, 0, 0, 5, 0, 1, 1)
+        ans = ptpmove(gbs_ptpmove, 1000, 0, 0, 0, 5, 0, 1, 1)
+        ##readlibrary(array)
+        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)+']'
+        ans = locktool(gbs_locktool)
+        ans = unlocktool(gbs_unlocktool)
+        ans = measuremove(gbs_measuremove, 1, 0, 0, 0, 0, 5, 0, 1, 1)
+        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))
+        ##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 = readlaser(gbs_readlaser)
+        print "Distance "+str(ans.distance)
+        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()
+

catkin_ws/src/gbs/scripts/test_client_synch_rs_v5.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, 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 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 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')
+    rospy.wait_for_service('gbs_readlaser')
+    rospy.wait_for_service('gbs_nailerdown')
+    rospy.wait_for_service('gbs_nailerup')
+    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)
+        gbs_readlaser = rospy.ServiceProxy('gbs_readlaser', GBSReadLaser)
+        gbs_nailerdown = rospy.ServiceProxy('gbs_nailerdown', GBSNailerDown)
+        gbs_nailerup = rospy.ServiceProxy('gbs_nailerup', GBSNailerUp)
+
+
+
+        ans = linmove(gbs_linmove, 2, 0, -0.6, math.radians(0), math.radians(0), math.radians(-175), 1, 1)
+        ans = linmove(gbs_linmove, 0, 0, -0.6, math.radians(0), math.radians(0), math.radians(-175), 1, 1)
+        ans = ptpmove(gbs_ptpmove, 2000, 0, 0, 0, 5, 0, 1, 1)
+        ans = ptpmove(gbs_ptpmove, 1000, 0, 0, 0, 5, 0, 1, 1)
+        ##readlibrary(array)
+        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)+']'
+        ans = locktool(gbs_locktool)
+        ans = unlocktool(gbs_unlocktool)
+        ans = measuremove(gbs_measuremove, 1, 0, 0, 0, 0, 5, 0, 1, 1)
+        ans = naillinmove(gbs_naillinmove, 1, 1, 1, 1, 0, 0, -0.6, math.radians(0), math.radians(0), math.radians(-175