Commit b4f3366b authored by Mathias Haage's avatar Mathias Haage
Browse files

Update to v5

parent 91164c0b

catkin_ws/src/gbs/scripts/synch_gbs_server_rs_v3.py

+19 −6
Original line number Diff line number Diff line
@@ -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/synch_gbs_server_rs_v4.py

0 → 100755
+521 −0
Original line number Diff line number Diff line 
#!/usr/bin/env python



from gbs.srv import *

import rospy

import socket

import struct

import sys

import argparse



HOST = '192.168.125.1'

PORT = 2500





DONOTCONNECT = False 





class Array:

    n = 0

    array = [0 for i in range(100)]





class Comm:



    def connect(self, host, port):

        if DONOTCONNECT:

            return

        self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)

        print "Connecting to %s %s"%(host, port)

        self.s.connect((host, port))



        self.packer = struct.Struct('!f')



    def close(self):

        if DONOTCONNECT:

            return

        self.s.close()



    def sendFloat(self, num):

        if DONOTCONNECT:

            return

        print "Sending float %f"%(num)

        data = self.packer.pack(num)

        self.s.sendall(data)



    def receiveFloat(self):

        if DONOTCONNECT:

            return 0.0

        data = self.s.recv(self.packer.size)

        tupl = self.packer.unpack(data)

        print "Received float %f"%(tupl[0])

        return tupl[0]



    def sendArray(self, array):

        if DONOTCONNECT:

            return

        for i in range(array.n):

            self.sendFloat(array.array[i])

        self.sendFloat(0.0)



    def receiveArray(self, n):

        array = Array()

        if DONOTCONNECT:

            return array

        for i in range(n):

            array.array[i] = self.receiveFloat()

        array.n = n

        return array



comm = Comm()



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]

       if 2+i < array.n:

           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,2) and near(c2,1):

           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,3) and near(c2,1):

           i = i + 3

           print "noop"

       elif near(c0,2) and near(c1,4) and near(c2,1):

           i = i + 3

           print "readlibrary"

       elif near(c0,2) and near(c1,5) and near(c2,1):

           i = i + 3

           print "readcurrentrobotpos"

       elif near(c0,2) and near(c1,6) and near(c2,1):

           i = i + 3

           print "locktool"

       elif near(c0,2) and near(c1,7) and near(c2,1):

           i = i + 3

           print "unlocktool"

       elif near(c0,17) and near(c1,8):

           id = array.array[2+i]

           bstop = array.array[3+i]

           s = array.array[4+i]

           ac = array.array[5+i]

           x1 = array.array[6+i]

           y1 = array.array[7+i]

           z1 = array.array[8+i]

           c1 = array.array[9+i]

           b1 = array.array[10+i]

           a1 = array.array[11+i]

           x2 = array.array[12+i]

           y2 = array.array[13+i]

           z2 = array.array[14+i]

           c2 = array.array[15+i]

           b2 = array.array[16+i]

           a2 = array.array[17+i]

           i = i + 18

           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,10) and near(c1,9):

           id = array.array[2+i]

           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 "measuremove id=%f j1=%f j2=%f j3=%f j4=%f j5=%f j6=%f s=%f ac=%f"%(id,j1,j2,j3,j4,j5,j6,s,ac)

       elif near(c0,16) and near(c1,10) and near(c2,1):

           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(c1,11) and near(c2,1):

           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,12) and near(c2,1):

           rpm = array.array[3+i]

           i = i + 4

           print "runspindle rpm=%f"%(rpm)

       elif near(c0,2) and near(c1,13) and near(c2,1):

           i = i + 3

           print "stopspindle"

       elif near(c0,2) and near(c1,14) and near(c2,1):

           i = i + 3

           print "lockendeffector"

       elif near(c0,2) and near(c1,15) and near(c2,1):

           i = i + 3

           print "unlockendeffector"

       elif near(c0,2) and near(c1,16) and near(c2,1):

           i = i + 3

           print "getmeasure"

       elif near(c0,2) and near(c1,17) and near(c2,1):

           i = i + 3

           print "lockpanel"

       elif near(c0,2) and near(c1,18) and near(c2,1):

           i = i + 3

           print "unlockpanel"

       elif near(c0,0) and near(c1,19):

           i = i + 2

           print "readlaser"

       elif near(c0,2) and near(c1,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,10,1, 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)

    comm.sendArray(array)

    ans = GBSCircMoveResponse()

    #ans.err1 = comm.receiveFloat()

    #comm.receiveFloat()

    ans.err1 = 0

    ans.err2 = comm.receiveFloat()

    comm.receiveFloat()

    return ans



def handle_gbs_combmove(req):

    array = Array()

    array.n = 3 + req.n

    array.array = [array.n-1,11,1] + [-1 for i in range(req.n + 1)]

    for i in range(req.n):

        array.array[3 + i] = req.array[i]

    prettyprint(array)

    comm.sendArray(array)

    ans = GBSCombMoveResponse()

    #ans.err1 = comm.receiveFloat()

    #comm.receiveFloat()

    ans.err1 = 0

    ans.err2 = comm.receiveFloat()

    comm.receiveFloat()

    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)

    comm.sendArray(array)

    ans = GBSLinMoveResponse()

    #ans.err1 = comm.receiveFloat()

    #comm.receiveFloat()

    ans.err1 = 0

    ans.err2 = comm.receiveFloat()

    comm.receiveFloat()

    return ans



def handle_gbs_lockspindle(req):

    array = Array()

    array.n = 3

    array.array = [2,14,1]

    prettyprint(array)

    comm.sendArray(array)

    ans = GBSLockSpindleResponse()

    ans.err = comm.receiveFloat()

    comm.receiveFloat()

    return ans



def handle_gbs_locktool(req):

    array = Array()

    array.n = 3

    array.array = [2,6,1]

    prettyprint(array)

    comm.sendArray(array)

    ans = GBSLockToolResponse()

    ans.err = comm.receiveFloat()

    comm.receiveFloat()

    return ans



def handle_gbs_measuremove(req):

    array = Array()

    array.n = 11

    array.array = [10,9, 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)

    comm.sendArray(array)

    ans = GBSMeasureMoveResponse()

    #ans.err1 = comm.receiveFloat()

    #comm.receiveFloat()

    ans.err1 = 0

    ans.err2 = comm.receiveFloat()

    comm.receiveFloat()

    return ans



def handle_gbs_naillinmove(req):

    array = Array()

    array.n = 18

    array.array = [17,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)

    comm.sendArray(array)

    ans = GBSNailLinMoveResponse()

    #ans.err1 = comm.receiveFloat()

    #comm.receiveFloat()

    ans.err1 = 0

    ans.err2 = comm.receiveFloat()

    comm.receiveFloat()

    return ans



def handle_gbs_ptpmove(req):

    array = Array()

    array.n = 11

    array.array = [10,2,1, req.p.j1, req.p.j2, req.p.j3, req.p.j4, req.p.j5, req.p.j6, req.s, req.ac]

    prettyprint(array)

    comm.sendArray(array)

    ans = GBSPTPMoveResponse()

    #ans.err1 = comm.receiveFloat()

    #comm.receiveFloat()

    ans.err1 = 0

    ans.err2 = comm.receiveFloat()

    comm.receiveFloat()

    return ans



def handle_gbs_readcurjointpose(req):

    array = Array()

    array.n = 3

    array.array = [2,5,1]

    prettyprint(array)

    comm.sendArray(array)

    ans = GBSReadCurJointPoseResponse()

    ans.err = comm.receiveFloat()

    comm.receiveFloat()

    ans.p.j1 = comm.receiveFloat()

    ans.p.j2 = comm.receiveFloat()

    ans.p.j3 = comm.receiveFloat()

    ans.p.j4 = comm.receiveFloat()

    ans.p.j5 = comm.receiveFloat()

    ans.p.j6 = comm.receiveFloat()

    return ans



def handle_gbs_releasepanel(req):

    array = Array()

    array.n = 3

    array.array = [2,18,1]

    prettyprint(array)

    comm.sendArray(array)

    ans = GBSReleasePanelResponse()

    ans.err = comm.receiveFloat()

    comm.receiveFloat()

    return ans



def handle_gbs_runspindle(req):

    array = Array()

    array.n = 4

    array.array = [3,12,1, req.rpm]

    prettyprint(array)

    comm.sendArray(array)

    ans = GBSRunSpindleResponse()

    err = comm.receiveFloat()

    comm.receiveFloat()

    return ans



def handle_gbs_readsensor(req):

    array = Array()

    array.n = 3

    array.array = [2,16,1]

    prettyprint(array)

    comm.sendArray(array)

    ans = GBSReadSensorResponse()

    ans.err = comm.receiveFloat()

    comm.receiveFloat()

    ans.data.x1 = comm.receiveFloat()

    ans.data.x2 = comm.receiveFloat()

    ans.data.y1 = comm.receiveFloat()

    return ans



def handle_gbs_shakevacuum(req):

    array = Array()

    array.n = 3

    array.array = [2,20,1]

    prettyprint(array)

    comm.sendArray(array)

    ans = GBSShakeVacuumResponse()

    ans.err = comm.receiveFloat()

    comm.receiveFloat()

    return ans



def handle_gbs_stopspindle(req):

    array = Array()

    array.n = 3

    array.array = [2,13,1]

    prettyprint(array)

    comm.sendArray(array)

    ans = GBSStopSpindleResponse()

    ans.err = comm.receiveFloat()

    comm.receiveFloat()

    return ans



def handle_gbs_switchvacuum(req):

    array = Array()

    array.n = 3

    array.array = [2,19,1]

    prettyprint(array)

    comm.sendArray(array)

    ans = GBSSwitchVacuumResponse()

    ans.err = comm.receiveFloat()

    comm.receiveFloat()

    return ans



def handle_gbs_takepanel(req):

    array = Array()

    array.n = 3

    array.array = [2,17,1]

    prettyprint(array)

    comm.sendArray(array)

    ans = GBSTakePanelResponse()

    ans.err = comm.receiveFloat()

    comm.receiveFloat()

    return ans



def handle_gbs_unlockspindle(req):

    array = Array()

    array.n = 3

    array.array = [2,15,1]

    prettyprint(array)

    comm.sendArray(array)

    ans = GBSUnlockSpindleResponse()

    ans.err = comm.receiveFloat()

    comm.receiveFloat()

    return ans



def handle_gbs_unlocktool(req):

    array = Array()

    array.n = 3

    array.array = [2,7,1]

    prettyprint(array)

    comm.sendArray(array)

    ans = GBSUnlockToolResponse()

    ans.err = comm.receiveFloat()

    comm.receiveFloat()

    return ans



def handle_gbs_readlaser(req):

    array = Array()

    array.n = 2

    array.array = [0,19]

    prettyprint(array)

    comm.sendArray(array)

    ans = GBSReadLaserResponse()

    ans.err = comm.receiveFloat()

    comm.receiveFloat()

    ans.distance = comm.receiveFloat()

    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)

    rospy.Service('gbs_readlaser', GBSReadLaser, handle_gbs_readlaser)

    print "Ready to receive GBS commands"

    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/synch_gbs_server_rs_v5.py

0 → 100755
+551 −0

File added.

Preview size limit exceeded, changes collapsed.

catkin_ws/src/gbs/scripts/test_client_synch_rs_v4.py

0 → 100755
+264 −0

File added.

Preview size limit exceeded, changes collapsed.

catkin_ws/src/gbs/scripts/test_client_synch_rs_v5.py

0 → 100755
+285 −0

File added.

Preview size limit exceeded, changes collapsed.