Skip to content
GitLab
Menu
Projects
Groups
Snippets
Loading...
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
Mathias Haage
GBS
Commits
a4e67861
Commit
a4e67861
authored
Jul 26, 2015
by
Mathias Haage
Browse files
Add scripts with RobotStudio connection and linmove working
parent
a2a33023
Changes
2
Hide whitespace changes
Inline
Side-by-side
catkin_ws/src/gbs/scripts/synch_gbs_server_rs_v3.py
0 → 100755
View file @
a4e67861
#!/usr/bin/env python
from
gbs.srv
import
*
import
rospy
import
socket
import
struct
import
sys
class
Array
:
n
=
0
array
=
[
0
for
i
in
range
(
100
)]
class
Comm
:
HOST
=
'vm100.cs.lth.se'
PORT
=
2500
def
connect
(
self
):
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
))
self
.
packer
=
struct
.
Struct
(
'!f'
)
def
close
(
self
):
self
.
s
.
close
()
def
sendFloat
(
self
,
num
):
print
"Sending float %f"
%
(
num
)
data
=
self
.
packer
.
pack
(
num
)
self
.
s
.
sendall
(
data
)
def
receiveFloat
(
self
):
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
):
for
i
in
range
(
array
.
n
):
self
.
sendFloat
(
array
.
array
[
i
])
def
receiveArray
(
self
,
n
):
array
=
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
]
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
,
1
)
and
near
(
c2
,
2
):
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
,
1
)
and
near
(
c2
,
4
):
i
=
i
+
3
print
"readlibrary"
elif
near
(
c0
,
2
)
and
near
(
c1
,
1
)
and
near
(
c2
,
5
):
i
=
i
+
3
print
"readcurrentrobotpos"
elif
near
(
c0
,
2
)
and
near
(
c1
,
1
)
and
near
(
c2
,
6
):
i
=
i
+
3
print
"locktool"
elif
near
(
c0
,
2
)
and
near
(
c1
,
1
)
and
near
(
c2
,
7
):
i
=
i
+
3
print
"unlocktool"
elif
near
(
c0
,
18
)
and
near
(
c1
,
1
)
and
near
(
c2
,
8
):
id
=
array
.
array
[
3
+
i
]
bstop
=
array
.
array
[
4
+
i
]
s
=
array
.
array
[
5
+
i
]
ac
=
array
.
array
[
6
+
i
]
x1
=
array
.
array
[
7
+
i
]
y1
=
array
.
array
[
8
+
i
]
z1
=
array
.
array
[
9
+
i
]
c1
=
array
.
array
[
10
+
i
]
b1
=
array
.
array
[
11
+
i
]
a1
=
array
.
array
[
12
+
i
]
x2
=
array
.
array
[
13
+
i
]
y2
=
array
.
array
[
14
+
i
]
z2
=
array
.
array
[
15
+
i
]
c2
=
array
.
array
[
16
+
i
]
b2
=
array
.
array
[
17
+
i
]
a2
=
array
.
array
[
18
+
i
]
i
=
i
+
19
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
,
11
)
and
near
(
c1
,
1
)
and
near
(
c2
,
8
):
id
=
array
.
array
[
3
+
i
]
x
=
array
.
array
[
4
+
i
]
y
=
array
.
array
[
5
+
i
]
z
=
array
.
array
[
6
+
i
]
c
=
array
.
array
[
7
+
i
]
b
=
array
.
array
[
8
+
i
]
a
=
array
.
array
[
9
+
i
]
s
=
array
.
array
[
10
+
i
]
ac
=
array
.
array
[
11
+
i
]
i
=
i
+
12
print
"measuremove id=%f x=%f y=%f z=%f c=%f b=%f a=%f s=%f ac=%f"
%
(
id
,
x
,
y
,
z
,
c
,
b
,
a
,
s
,
ac
)
elif
near
(
c0
,
16
)
and
near
(
c1
,
1
)
and
near
(
c2
,
10
):
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
(
c0
,
35
)
and
near
(
c1
,
1
)
and
near
(
c2
,
11
):
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
,
1
)
and
near
(
c2
,
12
):
rpm
=
array
.
array
[
3
+
i
]
i
=
i
+
4
print
"runspindle rpm=%f"
%
(
rpm
)
elif
near
(
c0
,
2
)
and
near
(
c1
,
1
)
and
near
(
c2
,
13
):
i
=
i
+
3
print
"stopspindle"
elif
near
(
c0
,
2
)
and
near
(
c1
,
1
)
and
near
(
c2
,
14
):
i
=
i
+
3
print
"lockspindle"
elif
near
(
c0
,
2
)
and
near
(
c1
,
1
)
and
near
(
c2
,
15
):
i
=
i
+
3
print
"unlockspindle"
elif
near
(
c0
,
2
)
and
near
(
c1
,
1
)
and
near
(
c2
,
16
):
i
=
i
+
3
print
"getsensorvalues"
elif
near
(
c0
,
2
)
and
near
(
c1
,
1
)
and
near
(
c2
,
17
):
i
=
i
+
3
print
"takepanel"
elif
near
(
c0
,
2
)
and
near
(
c1
,
1
)
and
near
(
c2
,
18
):
i
=
i
+
3
print
"releasepanel"
elif
near
(
c0
,
2
)
and
near
(
c1
,
1
)
and
near
(
c2
,
19
):
i
=
i
+
3
print
"switchvacuum"
elif
near
(
c0
,
2
)
and
near
(
c1
,
1
)
and
near
(
c2
,
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
,
1
,
10
,
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
.
err2
=
comm
.
receiveFloat
()
comm
.
receiveFloat
()
return
ans
def
handle_gbs_combmove
(
req
):
array
=
Array
()
array
.
n
=
3
+
req
.
n
array
.
array
=
[
35
,
1
,
11
]
+
[
-
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
.
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
,
1
,
14
]
prettyprint
(
array
)
comm
.
sendArray
(
array
)
ans
=
GBSLockSpindleResponse
()
ans
.
err
=
com
.
receiveFloat
()
comm
.
receiveFloat
()
return
ans
def
handle_gbs_locktool
(
req
):
array
=
Array
()
array
.
n
=
3
array
.
array
=
[
2
,
1
,
6
]
prettyprint
(
array
)
comm
.
sendArray
(
array
)
ans
=
GBSLockToolResponse
()
ans
.
err
=
com
.
receiveFloat
()
comm
.
receiveFloat
()
return
ans
def
handle_gbs_measuremove
(
req
):
array
=
Array
()
array
.
n
=
12
array
.
array
=
[
11
,
1
,
8
,
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
()
ans
=
GBSMeasureMoveResponse
()
ans
.
err1
=
comm
.
receiveFloat
()
comm
.
receiveFloat
()
ans
.
err2
=
comm
.
receiveFloat
()
comm
.
receiveFloat
()
return
ans
def
handle_gbs_naillinmove
(
req
):
array
=
Array
()
array
.
n
=
19
array
.
array
=
[
18
,
1
,
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
.
err2
=
comm
.
receiveFloat
()
comm
.
receiveFloat
()
return
ans
def
handle_gbs_ptpmove
(
req
):
array
=
Array
()
array
.
n
=
11
array
.
array
=
[
10
,
1
,
2
,
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
.
err2
=
comm
.
receiveFloat
()
comm
.
receiveFloat
()
return
ans
def
handle_gbs_readcurjointpose
(
req
):
array
=
Array
()
array
.
n
=
3
array
.
array
=
[
2
,
1
,
5
]
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
,
1
,
18
]
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
,
1
,
12
,
req
.
rpm
]
prettyprint
(
array
)
comm
.
sendArray
(
array
)
ans
=
GBSRunSpindleResponse
()
return
ans
def
handle_gbs_readsensor
(
req
):
array
=
Array
()
array
.
n
=
3
array
.
array
=
[
2
,
1
,
16
]
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
,
1
,
20
]
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
,
1
,
13
]
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
,
1
,
19
]
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
,
1
,
17
]
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
,
1
,
15
]
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
,
1
,
7
]
prettyprint
(
array
)
ans
=
GBSUnlockToolResponse
()
ans
.
err
=
comm
.
receiveFloat
()
comm
.
receiveFloat
()
return
ans
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
)
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
)
print
"Ready to receive GBS commands"
rospy
.
spin
()
if
__name__
==
"__main__"
:
add_services
()
catkin_ws/src/gbs/scripts/test_client_synch_rs_v3.py
0 → 100755
View file @
a4e67861
#!/usr/bin/env python
from
gbs.srv
import
*
import
rospy
import
socket
import
struct
import
sys
class
Array
:
n
=
0
array
=
[
0
for
i
in
range
(
100
)]
class
Comm
:
HOST
=
'vm100.cs.lth.se'
PORT
=
2500
def
connect
(
self
):
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
))
self
.
packer
=
struct
.
Struct
(
'!f'
)
def
close
(
self
):
self
.
s
.
close
()
def
sendFloat
(
self
,
num
):
print
"Sending float %f"
%
(
num
)
data
=
self
.
packer
.
pack
(
num
)
self
.
s
.
sendall
(
data
)
def
receiveFloat
(
self
):
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
):
for
i
in
range
(
array
.
n
):
self
.
sendFloat
(
array
.
array
[
i
])
def
receiveArray
(
self
,
n
):
array
=
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
]
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
,
1
)
and
near
(
c2
,
2
):
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
,
1
)
and
near
(
c2
,
4
):
i
=
i
+
3
print
"readlibrary"
elif
near
(
c0
,
2
)
and
near
(
c1
,
1
)
and
near
(
c2
,
5
):
i
=
i
+
3
print
"readcurrentrobotpos"
elif
near
(
c0
,
2
)
and
near
(
c1
,
1
)
and
near
(
c2
,
6
):
i
=
i
+
3
print
"locktool"
elif
near
(
c0
,
2
)
and
near
(
c1
,
1
)
and
near
(
c2
,
7
):
i
=
i
+
3
print
"unlocktool"
elif
near
(
c0
,
18
)
and
near
(
c1
,
1
)
and
near
(
c2
,
8
):
id
=
array
.
array
[
3
+
i
]
bstop
=
array
.
array
[
4
+
i
]