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
d9c7958b
Commit
d9c7958b
authored
Jul 22, 2015
by
Mathias Haage
Browse files
Add synchronous ros services
parent
3e95cb36
Changes
32
Hide whitespace changes
Inline
Side-by-side
catkin_ws/src/gbs/CMakeLists.txt
View file @
d9c7958b
...
...
@@ -47,13 +47,42 @@ find_package(catkin REQUIRED COMPONENTS
## Generate messages in the 'msg' folder
add_message_files
(
FILES
GBS
CmdArray
.msg
GBS
Acc
.msg
GBSAckArray.msg
GBSAns.msg
GBSBStop.msg
GBSCmdArray.msg
GBSEuler.msg
GBSId.msg
GBSJointPose.msg
GBSPose.msg
GBSPos.msg
GBSRPM.msg
GBSSensor.msg
GBSSpeed.msg
)
## Generate services in the 'srv' folder
add_service_files
(
FILES
GBSCircMove.srv
GBSCombMove.srv
GBSLinMove.srv
GBSLockSpindle.srv
GBSLockTool.srv
GBSMeasureMove.srv
GBSNailLinMove.srv
GBSPTPMove.srv
GBSReadCurJointPose.srv
GBSReadSensor.srv
GBSReleasePanel.srv
GBSRunSpindle.srv
GBSShakeVacuum.srv
GBSStopSpindle.srv
GBSSwitchVacuum.srv
GBSTakePanel.srv
GBSUnlockSpindle.srv
GBSUnlockTool.srv
PostGBSCmdArray.srv
)
...
...
catkin_ws/src/gbs/msg/GBSAcc.msg
0 → 100644
View file @
d9c7958b
float32 ac
catkin_ws/src/gbs/msg/GBSAns.msg
0 → 100644
View file @
d9c7958b
float32 err
catkin_ws/src/gbs/msg/GBSBStop.msg
0 → 100644
View file @
d9c7958b
float32 bstop
catkin_ws/src/gbs/msg/GBSEuler.msg
0 → 100644
View file @
d9c7958b
float32 c
float32 b
float32 a
catkin_ws/src/gbs/msg/GBSId.msg
0 → 100644
View file @
d9c7958b
float32 id
catkin_ws/src/gbs/msg/GBSJointPose.msg
0 → 100644
View file @
d9c7958b
float32 j1
float32 j2
float32 j3
float32 j4
float32 j5
float32 j6
catkin_ws/src/gbs/msg/GBSPos.msg
0 → 100644
View file @
d9c7958b
float32 x
float32 y
float32 z
catkin_ws/src/gbs/msg/GBSPose.msg
0 → 100644
View file @
d9c7958b
GBSPos pos
GBSEuler ori
catkin_ws/src/gbs/msg/GBSRPM.msg
0 → 100644
View file @
d9c7958b
float32 rpm
catkin_ws/src/gbs/msg/GBSSensor.msg
0 → 100644
View file @
d9c7958b
float32 x1
float32 x2
float32 y1
catkin_ws/src/gbs/msg/GBSSpeed.msg
0 → 100644
View file @
d9c7958b
float32 s
catkin_ws/src/gbs/scripts/synch_gbs_server.py
0 → 100755
View file @
d9c7958b
#!/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
)
print
"Ready to receive GBS commands"
rospy
.
spin
()
if
__name__
==
"__main__"
:
add_services
()
catkin_ws/src/gbs/scripts/test_client_synch.py
0 → 100755
View file @
d9c7958b
#!/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
=
s
req
.
ac
.
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
=
s
req
.
ac
.
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
=
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
=
s
req
.
ac
.
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
=
id
req
.
bstop
.
bstop
=
bstop
req
.
s
.
s
=
s
req
.
ac
.
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