Newer
Older
import roslib; roslib.load_manifest('ur5_driver')
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
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
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
import time, sys
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
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)
server.handle_request()
if __name__ == '__main__': main()