Newer
Older
import roslib; roslib.load_manifest('ur5_driver')
import time, sys, threading
import socket
import struct
import SocketServer
HOSTNAME='ur-xx'
HOSTNAME="10.0.1.20"
PORT=30001
MSG_OUT = 1
MSG_QUIT = 2
MSG_JOINT_STATES = 3
MULT_jointstate = 1000.0
class EOF(Exception): pass
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
# Only handles JOINT_STATES messages.
class StateTCPHandler(SocketServer.BaseRequestHandler):
def recv_more(self):
more = self.request.recv(4096)
if not more:
raise EOF()
return more
def handle(self):
print "Handling a joint states connection"
try:
buf = self.recv_more()
if not buf: return
while True:
print "Buf:", [ord(b) for b in buf]
# Unpacks the message type
mtype = struct.unpack_from("!i", buf, 0)[0]
buf = buf[4:]
print "Message type:", mtype
if mtype == MSG_OUT:
# Unpacks string message, terminated by tilde
i = buf.find("~")
while i < 0:
buf = buf + self.recv_more()
i = buf.find("~")
if len(buf) > 2000:
raise Exception("Probably forgot to terminate a string: %s..." % buf[:150])
s, buf = buf[:i], buf[i+1:]
print "Out: %s" % s
elif mtype == MSG_JOINT_STATES:
while len(buf) < 3*(6*4):
buf = buf + self.recv_more()
state_mult = struct.unpack_from("!" + ("i"*3*6), buf, 0)
buf = buf[3*6*4:]
state = [s / MULT_jointstate for s in state_mult]
print "Joint state:", state
else:
raise Exception("Unknown message type: %i" % mtype)
if not buf:
buf = buf + self.recv_more()
except EOF:
print "Connection closed"
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
class CommanderTCPHandler(SocketServer.BaseRequestHandler):
def recv_more(self):
more = self.request.recv(4096)
if not more:
raise EOF()
return more
def handle(self):
print "Handling a request"
try:
buf = self.recv_more()
if not buf: return
while True:
print "Buf:", [ord(b) for b in buf]
# Unpacks the message type
mtype = struct.unpack_from("!i", buf, 0)[0]
buf = buf[4:]
print "Message type:", mtype
if mtype == MSG_OUT:
# Unpacks string message, terminated by tilde
i = buf.find("~")
while i < 0:
buf = buf + self.recv_more()
i = buf.find("~")
if len(buf) > 2000:
raise Exception("Probably forgot to terminate a string: %s..." % buf[:150])
s, buf = buf[:i], buf[i+1:]
print "Out: %s" % s
elif mtype == MSG_QUIT:
print "Quitting"
sys.exit(0)
elif mtype == MSG_JOINT_STATES:
while len(buf) < 3*(6*4):
buf = buf + self.recv_more()
state_mult = struct.unpack_from("!" + ("i"*3*6), buf, 0)
buf = buf[3*6*4:]
state = [s / MULT_jointstate for s in state_mult]
print "Joint state:", state
else:
raise Exception("Unknown message type: %i" % mtype)
if not buf:
buf = buf + self.recv_more()
except EOF:
print "Connection closed"
class TCPServer(SocketServer.TCPServer):
allow_reuse_address = True
timeout = 5
sock = None
def main():
global sock
# Sends the program to the robot
sock = socket.create_connection((HOSTNAME, PORT))
with open('prog') as fin:
sock.sendall(fin.read())
server = TCPServer(("", 50001), CommanderTCPHandler)
server2 = TCPServer(("", 50002), StateTCPHandler)
thread_commander = threading.Thread(name="CommanderHandler", target=server.handle_request)
thread_state = threading.Thread(name="StateHandler", target=server2.handle_request)
thread_state.daemon = True
thread_commander.start()
thread_state.start()
thread_commander.join()
thread_state.join()
print "Dump"
print "="*70
o = ""
while len(o) < 4096:
r = sock.recv(1024)
if not r: break
o = o + r
print o
if __name__ == '__main__': main()