Commit b4f3366b authored by Mathias Haage's avatar Mathias Haage

Update to v5

parent 91164c0b
...@@ -5,7 +5,12 @@ import rospy ...@@ -5,7 +5,12 @@ import rospy
import socket import socket
import struct import struct
import sys 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: class Array:
n = 0 n = 0
...@@ -13,13 +18,11 @@ class Array: ...@@ -13,13 +18,11 @@ class Array:
class Comm: 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) self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
print "Connecting to %s %s"%(self.HOST, self.PORT) print "Connecting to %s %s"%(host, port)
self.s.connect((self.HOST, self.PORT)) self.s.connect((host, port))
self.packer = struct.Struct('!f') self.packer = struct.Struct('!f')
...@@ -40,6 +43,7 @@ class Comm: ...@@ -40,6 +43,7 @@ class Comm:
def sendArray(self, array): def sendArray(self, array):
for i in range(array.n): for i in range(array.n):
self.sendFloat(array.array[i]) self.sendFloat(array.array[i])
self.sendFloat(0.0)
def receiveArray(self, n): def receiveArray(self, n):
array = Array() array = Array()
...@@ -452,7 +456,6 @@ def handle_gbs_unlocktool(req): ...@@ -452,7 +456,6 @@ def handle_gbs_unlocktool(req):
def add_services(): def add_services():
comm.connect()
rospy.init_node('synch_gbs_server') rospy.init_node('synch_gbs_server')
rospy.Service('gbs_circmove', GBSCircMove, handle_gbs_circmove) rospy.Service('gbs_circmove', GBSCircMove, handle_gbs_circmove)
rospy.Service('gbs_combmove', GBSCombMove, handle_gbs_combmove) rospy.Service('gbs_combmove', GBSCombMove, handle_gbs_combmove)
...@@ -476,5 +479,15 @@ def add_services(): ...@@ -476,5 +479,15 @@ def add_services():
rospy.spin() rospy.spin()
if __name__ == "__main__": 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() add_services()
This diff is collapsed.
This diff is collapsed.
#!/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()
#!/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), 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 = shakevacuum(gbs_shakevacuum)
ans = readlaser(gbs_readlaser)
print "Distance "+str(ans.distance)
ans = nailerdown(gbs_nailerdown)
ans = nailerup(gbs_nailerup)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def usage():
return "%s".s.argv[0]
if __name__ == "__main__":
test()
---
float32 err
float32 distance
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment