Skip to content

GitLab

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

Update to v6

parent 05c8f1b8
Hide whitespace changes
Inline Side-by-side
catkin_ws/src/gbs/CMakeLists.txt
View file @ fdc16d64
......@@ -81,6 +81,7 @@ add_service_files(
GBSReadLaser.srv
GBSNailerDown.srv
GBSNailerUp.srv
GBSReadFlange.srv
)
## Generate actions in the 'action' folder
......
catkin_ws/src/gbs/scripts/synch_gbs_server_rs_v5.py
View file @ fdc16d64
......@@ -7,11 +7,12 @@ import struct
import sys
import argparse
HOST = '192.168.125.1'
#HOST = '192.168.125.1'
HOST = '130.235.16.100'
PORT = 2500
DONOTCONNECT = True
DONOTCONNECT = False
class Array:
......
catkin_ws/src/gbs/scripts/synch_gbs_server_rs_v6.py 0 → 100755
View file @ fdc16d64
#!/usr/bin/env python
from gbs.srv import *
import rospy
import socket
import struct
import sys
import argparse
#HOST = '192.168.125.1'
HOST = '130.235.16.100'
PORT = 2500
DONOTCONNECT = False
class Array:
n = 0
array = [0 for i in range(200)]
class Comm:
s = None
packer = struct.Struct('!f')
def connect(self, host, port):
self.host = host
self.port = port
if DONOTCONNECT:
return
if self.s == None:
try:
self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
print "Trying to connect to %s %s"%(host, port)
self.s.connect((host, port))
except socket.error:
self.s.close()
self.s = None
print "Could not connect"
def close(self):
if DONOTCONNECT:
return
if s != None:
self.s.close()
def sendFloat(self, num):
if DONOTCONNECT:
return
print "Sending float %f"%(num)
data = self.packer.pack(num)
if self.s != None:
try:
self.s.sendall(data)
except socket.error:
self.s.close()
self.s = None
print "Socket error"
def receiveFloat(self):
if DONOTCONNECT:
return 0.0
if self.s != None:
try:
self.data = self.s.recv(self.packer.size)
except socket.error:
self.s.close()
self.s = None
print "Socket error"
return 0
if len(self.data) != 4:
print "Wrong size of received float"
self.s.close()
self.s = None
return 0
tupl = self.packer.unpack(self.data)
print "Received float %f"%(tupl[0])
return tupl[0]
return 0
def sendArray(self, array):
if DONOTCONNECT:
return
if self.s == None:
self.connect(self.host, self.port)
if self.s != None:
for i in range(array.n):
self.sendFloat(array.array[i])
self.sendFloat(0.0)
else:
print "Skipping sending command"
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] # Length
c1 = array.array[1+i] # Cmd
if 2+i < array.n:
c2 = array.array[2+i] # Id
if near(c1,1): # CMD_MOVE_LIN
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(c1,2): # CMD_MOVE_PTP
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(c1,3): # CMD_NO_OPERATION
i = i + 3
print "noop"
elif near(c1,4): # CMD_READ_LIB
i = i + 3
print "readlibrary"
elif near(c1,5): # CMD_READ_PTP
i = i + 3
print "readcurrentrobotpos"
elif near(c1,6): # CMD_LCK_TL
i = i + 3
print "locktool"
elif near(c1,7): # CMD_UNLCK_TL
i = i + 3
print "unlocktool"
elif near(c1,8): # CMD_NAIL_LIN
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(c1,9): # CMD_MOVE_MEASURE
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(c1,10): # CMD_MOVE_ARC
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): # CMD_MOVE
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(c1,12): # CMD_RUN_SPINDLE
rpm = array.array[3+i]
i = i + 4
print "runspindle rpm=%f"%(rpm)
elif near(c1,13): # CMD_STOP_SPINDLE
i = i + 3
print "stopspindle"
elif near(c1,14): # CMD_LCK_ENDEFFECTOR
i = i + 3
print "lockendeffector"
elif near(c1,15): # CMD_UNLCK_ENDEFFECTOR
i = i + 3
print "unlockendeffector"
elif near(c1,16): # CMD_GET_MEASURE
i = i + 3
print "getmeasure"
elif near(c1,17): # CMD_LCK_PANEL
i = i + 3
print "lockpanel"
elif near(c1,18): # CMD_UNLCK_PANEL
i = i + 3
print "unlockpanel"
elif near(c1,19): # CMD_RUN_VACUUM
i = i + 3
print "readlaser"
elif near(c1,20): # CMD_SHAKE_VACUUM
i = i + 3
print "shakevacuum"
elif near(c1,21): # CMD_NAILER_DOWN
i = i + 3
print "nailerdown"
elif near(c1,22): # CMD_NAILER_UP
i = i + 3
print "nailerup"
elif near(c1,23): # CMD_READ_LASER
i = i + 2
print "readlaser"
elif near(c1,24): # CMD_READ_FLANGE
i = i + 2
print "readflange"
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 = [1,23]
prettyprint(array)
comm.sendArray(array)
ans = GBSReadLaserResponse()
ans.err = comm.receiveFloat()
comm.receiveFloat()
ans.distance = comm.receiveFloat()
return ans
def handle_gbs_nailerdown(req):
array = Array()
array.n = 3
array.array = [2,21,req.id]
prettyprint(array)
comm.sendArray(array)
ans = GBSNailerDownResponse()
ans.err = comm.receiveFloat()
comm.receiveFloat()
return ans
def handle_gbs_nailerup(req):
array = Array()
array.n = 3
array.array = [2,22,req.id]
prettyprint(array)
comm.sendArray(array)
ans = GBSNailerUpResponse()
ans.err = comm.receiveFloat()
comm.receiveFloat()
return ans
def handle_gbs_readflange(req):
array = Array()
array.n = 2