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
899f7672
Commit
899f7672
authored
Jul 23, 2015
by
Mathias Haage
Browse files
Remove scripts for previous versions
parent
60ccc941
Changes
4
Hide whitespace changes
Inline
Side-by-side
catkin_ws/src/gbs/scripts/post_gbs_cmdarray.py
deleted
100755 → 0
View file @
60ccc941
#!/usr/bin/env python
from
gbs.srv
import
*
import
rospy
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_post_gbs_cmdarray
(
req
):
print
"Received an GBS array"
prettyprint
(
req
.
array
)
return
PostGBSCmdArrayResponse
()
def
add_post_gbs_cmdarray
():
rospy
.
init_node
(
'post_gbs_cmdarray_server'
)
s
=
rospy
.
Service
(
'post_gbs_cmdarray'
,
PostGBSCmdArray
,
handle_post_gbs_cmdarray
)
print
"Ready to receive GBS arrays"
rospy
.
spin
()
if
__name__
==
"__main__"
:
add_post_gbs_cmdarray
()
catkin_ws/src/gbs/scripts/synch_gbs_server.py
deleted
100755 → 0
View file @
60ccc941
#!/usr/bin/env python
from
gbs.srv
import
*
import
rospy
class
Array
:
n
=
0
array
=
[
0
for
i
in
range
(
100
)]
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
.
s
,
req
.
ac
.
ac
]
prettyprint
(
array
)
ans
=
GBSCircMoveResponse
()
ans
.
ans1
.
err
=
0
ans
.
ans2
.
err
=
0
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
)
ans
=
GBSCombMoveResponse
()
ans
.
ans1
.
err
=
0
ans
.
ans2
.
err
=
0
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
.
s
,
req
.
ac
.
ac
]
prettyprint
(
array
)
ans
=
GBSLinMoveResponse
()
ans
.
ans1
.
err
=
0
ans
.
ans2
.
err
=
0
return
ans
def
handle_gbs_lockspindle
(
req
):
array
=
Array
()
array
.
n
=
3
array
.
array
=
[
2
,
1
,
14
]
prettyprint
(
array
)
ans
=
GBSLockSpindleResponse
()
ans
.
ans
.
err
=
0
return
ans
def
handle_gbs_locktool
(
req
):
array
=
Array
()
array
.
n
=
3
array
.
array
=
[
2
,
1
,
6
]
prettyprint
(
array
)
ans
=
GBSLockToolResponse
()
ans
.
ans
.
err
=
0
return
ans
def
handle_gbs_measuremove
(
req
):
array
=
Array
()
array
.
n
=
12
array
.
array
=
[
11
,
1
,
8
,
req
.
id
.
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
.
s
,
req
.
ac
.
ac
]
prettyprint
(
array
)
ans
=
GBSMeasureMoveResponse
()
ans
.
ans1
.
err
=
0
ans
.
ans2
.
err
=
0
return
ans
def
handle_gbs_naillinmove
(
req
):
array
=
Array
()
array
.
n
=
19
array
.
array
=
[
18
,
1
,
8
,
req
.
id
.
id
,
req
.
bstop
.
bstop
,
req
.
s
.
s
,
req
.
ac
.
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
)
ans
=
GBSNailLinMoveResponse
()
ans
.
ans1
.
err
=
0
ans
.
ans2
.
err
=
0
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
.
s
,
req
.
ac
.
ac
]
prettyprint
(
array
)
ans
=
GBSPTPMoveResponse
()
ans
.
ans1
.
err
=
0
ans
.
ans2
.
err
=
0
return
ans
def
handle_gbs_readcurjointpose
(
req
):
array
=
Array
()
array
.
n
=
3
array
.
array
=
[
2
,
1
,
5
]
prettyprint
(
array
)
ans
=
GBSReadCurJointPoseResponse
()
ans
.
ans
.
err
=
0
ans
.
p
.
j1
=
0
# ...
return
ans
def
handle_gbs_releasepanel
(
req
):
array
=
Array
()
array
.
n
=
3
array
.
array
=
[
2
,
1
,
18
]
prettyprint
(
array
)
ans
=
GBSReleasePanelResponse
()
ans
.
ans
.
err
=
0
return
ans
def
handle_gbs_runspindle
(
req
):
array
=
Array
()
array
.
n
=
4
array
.
array
=
[
3
,
1
,
12
,
req
.
rpm
.
rpm
]
prettyprint
(
array
)
ans
=
GBSRunSpindleResponse
()
return
ans
def
handle_gbs_readsensor
(
req
):
array
=
Array
()
array
.
n
=
3
array
.
array
=
[
2
,
1
,
16
]
prettyprint
(
array
)
ans
=
GBSReadSensorResponse
()
ans
.
ans
.
err
=
0
ans
.
data
.
x1
=
0
# ...
return
ans
def
handle_gbs_shakevacuum
(
req
):
array
=
Array
()
array
.
n
=
3
array
.
array
=
[
2
,
1
,
20
]
prettyprint
(
array
)
ans
=
GBSShakeVacuumResponse
()
ans
.
ans
.
err
=
0
return
ans
def
handle_gbs_stopspindle
(
req
):
array
=
Array
()
array
.
n
=
3
array
.
array
=
[
2
,
1
,
13
]
prettyprint
(
array
)
ans
=
GBSStopSpindleResponse
()
ans
.
ans
.
err
=
0
return
ans
def
handle_gbs_switchvacuum
(
req
):
array
=
Array
()
array
.
n
=
3
array
.
array
=
[
2
,
1
,
19
]
prettyprint
(
array
)
ans
=
GBSSwitchVacuumResponse
()
ans
.
ans
.
err
=
0
return
ans
def
handle_gbs_takepanel
(
req
):
array
=
Array
()
array
.
n
=
3
array
.
array
=
[
2
,
1
,
17
]
prettyprint
(
array
)
ans
=
GBSTakePanelResponse
()
ans
.
ans
.
err
=
0
return
ans
def
handle_gbs_unlockspindle
(
req
):
array
=
Array
()
array
.
n
=
3
array
.
array
=
[
2
,
1
,
15
]
prettyprint
(
array
)
ans
=
GBSUnlockSpindleResponse
()
ans
.
ans
.
err
=
0
return
ans
def
handle_gbs_unlocktool
(
req
):
array
=
Array
()
array
.
n
=
3
array
.
array
=
[
2
,
1
,
7
]
prettyprint
(
array
)
ans
=
GBSUnlockToolResponse
()
ans
.
ans
.
err
=
0
return
ans
def
add_services
():
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
)