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
a2a33023
Commit
a2a33023
authored
Jul 23, 2015
by
Mathias Haage
Browse files
Add scripts for version 3
parent
899f7672
Changes
2
Hide whitespace changes
Inline
Side-by-side
catkin_ws/src/gbs/scripts/synch_gbs_server_v3.py
0 → 100755
View file @
a2a33023
#!/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
,
req
.
ac
]
prettyprint
(
array
)
ans
=
GBSCircMoveResponse
()
ans
.
err1
=
0
ans
.
err2
=
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
.
err1
=
0
ans
.
err2
=
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
,
req
.
ac
]
prettyprint
(
array
)
ans
=
GBSLinMoveResponse
()
ans
.
err1
=
0
ans
.
err2
=
0
return
ans
def
handle_gbs_lockspindle
(
req
):
array
=
Array
()
array
.
n
=
3
array
.
array
=
[
2
,
1
,
14
]
prettyprint
(
array
)
ans
=
GBSLockSpindleResponse
()
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
.
err
=
0
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
)
ans
=
GBSMeasureMoveResponse
()
ans
.
err1
=
0
ans
.
err2
=
0
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
)
ans
=
GBSNailLinMoveResponse
()
ans
.
err1
=
0
ans
.
err2
=
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
,
req
.
ac
]
prettyprint
(
array
)
ans
=
GBSPTPMoveResponse
()
ans
.
err1
=
0
ans
.
err2
=
0
return
ans
def
handle_gbs_readcurjointpose
(
req
):
array
=
Array
()
array
.
n
=
3
array
.
array
=
[
2
,
1
,
5
]
prettyprint
(
array
)
ans
=
GBSReadCurJointPoseResponse
()
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
.
err
=
0
return
ans
def
handle_gbs_runspindle
(
req
):
array
=
Array
()
array
.
n
=
4
array
.
array
=
[
3
,
1
,
12
,
req
.
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
.
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
.
err
=
0
return
ans
def
handle_gbs_stopspindle
(
req
):
array
=
Array
()
array
.
n
=
3
array
.
array
=
[
2
,
1
,
13
]
prettyprint
(
array
)
ans
=
GBSStopSpindleResponse
()
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
.
err
=
0
return
ans
def
handle_gbs_takepanel
(
req
):
array
=
Array
()
array
.
n
=
3
array
.
array
=
[
2
,
1
,
17
]
prettyprint
(
array
)
ans
=
GBSTakePanelResponse
()
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
.
err
=
0
return
ans
def
handle_gbs_unlocktool
(
req
):
array
=
Array
()
array
.
n
=
3
array
.
array
=
[
2
,
1
,
7
]
prettyprint
(
array
)
ans
=
GBSUnlockToolResponse
()
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
)
print
"Ready to receive GBS commands"
rospy
.
spin
()
if
__name__
==
"__main__"
:
add_services
()
catkin_ws/src/gbs/scripts/test_client_synch_v3.py
0 → 100755
View file @
a2a33023
#!/usr/bin/env python
import
sys
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
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'
)