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
8b73f0a0
Commit
8b73f0a0
authored
Aug 07, 2015
by
Sven Gestegård Robertz
Browse files
initial labcomm client based on example_client
parent
884ce343
Changes
1
Hide whitespace changes
Inline
Side-by-side
catkin_ws/src/gbs/scripts/lc_client.py
0 → 100755
View file @
8b73f0a0
#!/usr/bin/python
import
labcomm2014
import
commands
import
sys
,
traceback
import
collections
import
threading
import
socket
import
rwsocket
COMMAND_RECEIVED
=
0
RESP_ACK
=
1
RESP_READ_PTP
=
2
RESP_MOVE_MEASURE
=
3
GBS_ERROR
=
-
1
WHAT
=
0
#placeholder to be checked/changed
class
ResponseMismatch
(
Exception
):
def
__init__
(
self
,
expected
,
received
):
self
.
expected
=
expected
self
.
received
=
received
def
__str__
(
self
):
return
"Unexpected response: expected %s, got %s"
%
(
self
.
expected
,
self
.
received
)
class
test_client
(
object
):
def
add_response
(
self
,
type
,
val
):
self
.
response
.
acquire
()
self
.
responses
.
append
((
type
,
val
))
self
.
response
.
notifyAll
()
self
.
response
.
release
()
raise
NotImplementedError
()
def
wait_response
(
self
,
type
):
self
.
response
.
acquire
()
while
len
(
self
.
responses
)
<
1
:
self
.
response
.
wait
()
(
t
,
res
)
=
self
.
responses
.
popleft
()
self
.
response
.
release
()
if
t
==
type
:
return
res
else
:
raise
ResponseMismatch
(
type
,
t
)
def
__init__
(
self
,
server
):
#conditions for awaiting GBS responses
self
.
response
=
threading
.
Condition
()
self
.
responses
=
collections
.
deque
()
self
.
d
=
labcomm2014
.
Decoder
(
labcomm2014
.
StreamReader
(
server
),
version
)
self
.
e
=
labcomm2014
.
Encoder
(
labcomm2014
.
StreamWriter
(
server
),
version
)
self
.
e
.
add_decl
(
commands
.
CMD_MOVE_LIN
.
signature
)
self
.
e
.
add_decl
(
commands
.
CMD_MOVE_PTP
.
signature
)
self
.
e
.
add_decl
(
commands
.
CMD_NO_OPERATION
.
signature
)
self
.
e
.
add_decl
(
commands
.
CMD_READ_LIB
.
signature
)
self
.
e
.
add_decl
(
commands
.
CMD_READ_PTP
.
signature
)
self
.
e
.
add_decl
(
commands
.
CMD_LCK_TL
.
signature
)
self
.
e
.
add_decl
(
commands
.
CMD_UNLCK_TL
.
signature
)
self
.
e
.
add_decl
(
commands
.
CMD_NAIL_LIN
.
signature
)
self
.
e
.
add_decl
(
commands
.
CMD_MOVE_MEASURE
.
signature
)
self
.
e
.
add_decl
(
commands
.
CMD_MOVE_ARC
.
signature
)
self
.
e
.
add_decl
(
commands
.
CMD_MOVE
.
signature
)
self
.
d
.
register_handler
(
commands
.
COMMAND_RECEIVED
.
signature
,
self
.
handle_COMMAND_RECEIVED
)
self
.
d
.
register_handler
(
commands
.
RESP_ACK
.
signature
,
self
.
handle_RESP_ACK
)
self
.
d
.
register_handler
(
commands
.
RESP_READ_PTP
.
signature
,
self
.
handle_RESP_READ_PTP
)
self
.
d
.
register_handler
(
commands
.
RESP_MOVE_MEASURE
.
signature
,
self
.
handle_RESP_MOVE_MEASURE
)
self
.
e
.
add_decl
(
commands
.
shutdown
.
signature
)
self
.
d
.
register_handler
(
commands
.
shutdown
.
signature
,
self
.
handle_shutdown
)
self
.
cmd_seq
=
0
self
.
run
=
True
def
handle_COMMAND_RECEIVED
(
self
,
val
):
print
"got COMMAND_RECEIVED: %s"
%
val
self
.
add_response
(
COMMAND_RECEIVED
,
val
)
def
handle_RESP_NAK
(
self
,
val
):
print
"got RESP_NAK: %s"
%
val
self
.
add_response
(
RESP_NAK
,
val
)
def
handle_RESP_ACK
(
self
,
val
):
print
"got RESP_ACK: %s"
%
val
self
.
add_response
(
RESP_ACK
,
val
)
def
handle_RESP_READ_PTP
(
self
,
val
):
print
"got RESP_READ_PTP: %s"
%
val
self
.
add_response
(
RESP_READ_PTP
,
val
)
def
handle_RESP_MOVE_MEASURE
(
self
,
val
):
print
"got RESP_MOVE_MEASURE: %s"
%
val
self
.
add_response
(
RESP_MOVE_MEASURE
,
val
)
def
handle_shutdown
(
self
,
val
):
print
"got shutdown: %s"
%
val
self
.
run
=
False
def
send_command
(
self
,
cmd
,
sig
):
self
.
e
.
encode
(
cmd
,
sig
)
self
.
cmd_seq
+=
1
def
shutdown
(
self
):
self
.
send_command
(
self
.
cmd_seq
,
commands
.
shutdown
.
signature
)
def
CMD_MOVE_LIN
(
self
,
coordinate
,
angle
,
speed
,
acc
):
self
.
send_command
(
dict
(
seq
=
self
.
cmd_seq
,
cmd
=
dict
(
pos
=
dict
(
pos
=
coordinate
,
rot
=
angle
),
S_Ac
=
dict
(
speed
=
speed
,
acc
=
acc
))),
commands
.
CMD_MOVE_LIN
.
signature
)
res
=
self
.
wait_response
(
COMMAND_RECEIVED
)
print
(
"... COMMAND_RECEIVED: %s"
%
res
)
res
=
self
.
wait_response
(
RESP_ACK
)
print
(
"... RESP_ACK: %s"
%
res
)
res
=
self
.
wait_response
(
RESP_ACK
)
print
(
"... RESP_ACK: %s"
%
res
)
return
WHAT
def
CMD_MOVE_PTP
(
self
,
j
,
s
,
a
):
self
.
send_command
(
dict
(
seq
=
self
.
cmd_seq
,
cmd
=
dict
(
j
=
j
,
S_Ac
=
dict
(
speed
=
s
,
acc
=
a
))),
commands
.
CMD_MOVE_PTP
.
signature
)
res
=
self
.
wait_response
(
COMMAND_RECEIVED
)
print
(
"... COMMAND_RECEIVED: %s"
%
res
)
res
=
self
.
wait_response
(
RESP_ACK
)
print
(
"... RESP_ACK: %s"
%
res
)
res
=
self
.
wait_response
(
RESP_ACK
)
print
(
"... RESP_ACK: %s"
%
res
)
return
WHAT
def
CMD_NO_OPERATION
(
self
):
self
.
send_command
(
self
.
cmd_seq
,
commands
.
CMD_NO_OPERATION
.
signature
)
def
CMD_READ_LIB
(
self
):
self
.
send_command
(
self
.
cmd_seq
,
commands
.
CMD_READ_LIB
.
signature
)
def
CMD_READ_PTP
(
self
):
self
.
send_command
(
self
.
cmd_seq
,
commands
.
CMD_READ_PTP
.
signature
)
res
=
self
.
wait_response
(
COMMAND_RECEIVED
)
print
(
"... COMMAND_RECEIVED: %s"
%
res
)
res
=
self
.
wait_response
(
RESP_READ_PTP
)
print
(
"... RESP_READ_PTP: %s"
%
res
)
return
res
.
val
def
CMD_LCK_TL
(
self
):
self
.
send_command
(
self
.
cmd_seq
,
commands
.
CMD_LCK_TL
.
signature
)
res
=
self
.
wait_response
(
COMMAND_RECEIVED
)
print
(
"... COMMAND_RECEIVED: %s"
%
res
)
res
=
self
.
wait_response
(
RESP_ACK
)
print
(
"... RESP_ACK: %s"
%
res
)
#
# ---
# float32 err
def
CMD_UNLCK_TL
(
self
):
self
.
send_command
(
self
.
cmd_seq
,
commands
.
CMD_UNLCK_TL
.
signature
)
res
=
self
.
wait_response
(
COMMAND_RECEIVED
)
print
(
"... COMMAND_RECEIVED: %s"
%
res
)
res
=
self
.
wait_response
(
RESP_ACK
)
print
(
"... RESP_ACK: %s"
%
res
)
def
CMD_NAIL_LIN
(
self
,
id
,
bstop
,
start
,
finish
,
speed
,
acc
):
self
.
send_command
(
dict
(
seq
=
self
.
cmd_seq
,
id
=
id
,
bstop
=
bstop
,
S_Ac
=
dict
(
speed
=
speed
,
acc
=
acc
),
start
=
start
,
finish
=
finish
),
commands
.
CMD_NAIL_LIN
.
signature
)
res
=
self
.
wait_response
(
COMMAND_RECEIVED
)
print
(
"... COMMAND_RECEIVED: %s"
%
res
)
res
=
self
.
wait_response
(
RESP_ACK
)
print
(
"... RESP_ACK: %s"
%
res
)
res
=
self
.
wait_response
(
RESP_ACK
)
print
(
"... RESP_ACK: %s"
%
res
)
def
CMD_MOVE_MEASURE
(
self
,
id
,
coordinate
,
angle
,
speed
,
acc
):
self
.
send_command
(
dict
(
seq
=
self
.
cmd_seq
,
id
=
id
,
pos
=
dict
(
pos
=
coordinate
,
rot
=
angle
),
S_Ac
=
dict
(
speed
=
speed
,
acc
=
acc
)),
commands
.
CMD_MOVE_MEASURE
.
signature
)
res
=
self
.
wait_response
(
COMMAND_RECEIVED
)
print
(
"... COMMAND_RECEIVED: %s"
%
res
)
res
=
self
.
wait_response
(
RESP_ACK
)
print
(
"... RESP_ACK: %s"
%
res
)
res
=
self
.
wait_response
(
RESP_ACK
)
print
(
"... RESP_ACK: %s"
%
res
)
res
=
self
.
wait_response
(
RESP_MOVE_MEASURE
)
print
(
"... RESP_MOVE_MEASURE: %s"
%
res
)
def
CMD_MOVE_ARC
(
self
,
posI
,
posF
,
speed
,
acc
):
self
.
send_command
(
dict
(
seq
=
self
.
cmd_seq
,
cmd
=
dict
(
posI
=
posI
,
posF
=
posF
,
S_Ac
=
dict
(
speed
=
speed
,
acc
=
acc
))),
commands
.
CMD_MOVE_ARC
.
signature
)
res
=
self
.
wait_response
(
COMMAND_RECEIVED
)
print
(
"... COMMAND_RECEIVED: %s"
%
res
)
res
=
self
.
wait_response
(
RESP_ACK
)
print
(
"... RESP_ACK: %s"
%
res
)
res
=
self
.
wait_response
(
RESP_ACK
)
print
(
"... RESP_ACK: %s"
%
res
)
#TODO
def
CMD_MOVE
(
self
,
moves
):
self
.
send_command
(
dict
(
seq
=
self
.
cmd_seq
,
cmd
=
moves
),
commands
.
CMD_MOVE
.
signature
)
def
run_one
(
self
):
self
.
d
.
runOne
();
return
self
.
run
def
receiver
(
client
):
run
=
True
while
run
:
run
=
client
.
run_one
()
if
__name__
==
"__main__"
:
port
=
sys
.
argv
[
1
]
if
len
(
sys
.
argv
)
>=
2
else
9876
version
=
sys
.
argv
[
2
]
if
len
(
sys
.
argv
)
==
3
else
"LabComm2014.v1"
server
=
socket
.
socket
(
socket
.
AF_INET
,
socket
.
SOCK_STREAM
)
server
.
connect
((
socket
.
gethostname
(),
port
))
client
=
test_client
(
server
)
t
=
threading
.
Thread
(
target
=
receiver
,
args
=
(
client
,))
t
.
start
()
try
:
client
.
CMD_UNLCK_TL
()
client
.
CMD_LCK_TL
()
coord
=
dict
(
x
=
0
,
y
=
1
,
z
=
2
)
rot
=
dict
(
c
=
0
,
b
=
0
,
a
=
0
)
client
.
CMD_MOVE_LIN
(
coord
,
rot
,
1
,
0.1
)
client
.
CMD_MOVE_PTP
(
range
(
6
),
1
,
0.1
)
client
.
CMD_READ_PTP
()
coord
=
dict
(
x
=
0
,
y
=
1
,
z
=
2
)
rot
=
dict
(
c
=
0
,
b
=
0
,
a
=
0
)
client
.
CMD_MOVE_MEASURE
(
1
,
coord
,
rot
,
1
,
0.1
)
start
=
dict
(
pos
=
dict
(
x
=
0
,
y
=
1
,
z
=
2
),
rot
=
dict
(
c
=
0
,
b
=
0
,
a
=
0
))
finish
=
dict
(
pos
=
dict
(
x
=
0
,
y
=
1
,
z
=
2
),
rot
=
dict
(
c
=
0
,
b
=
0
,
a
=
0
))
client
.
CMD_NAIL_LIN
(
1
,
False
,
start
,
finish
,
1
,
0.1
)
posI
=
dict
(
pos
=
dict
(
x
=
0
,
y
=
1
,
z
=
2
),
rot
=
dict
(
c
=
0
,
b
=
0
,
a
=
0
))
posF
=
dict
(
pos
=
dict
(
x
=
0
,
y
=
1
,
z
=
2
),
rot
=
dict
(
c
=
0
,
b
=
0
,
a
=
0
))
client
.
CMD_MOVE_ARC
(
posI
,
posF
,
1
,
0.1
)
#linear
move1
=
dict
(
pos
=
dict
(
pos
=
dict
(
x
=
0
,
y
=
1
,
z
=
2
),
rot
=
dict
(
c
=
0
,
b
=
0
,
a
=
0
)),
S_Ac
=
dict
(
speed
=
1
,
acc
=
2
))
move2
=
dict
(
pos
=
dict
(
pos
=
dict
(
x
=
5
,
y
=
1
,
z
=
0
),
rot
=
dict
(
c
=
0
,
b
=
0
,
a
=
0
)),
S_Ac
=
dict
(
speed
=
1
,
acc
=
2
))
#ptp
move3
=
dict
(
j
=
[
0
,
0
,
0
,
0
,
0
,
0
],
S_Ac
=
dict
(
speed
=
0.1
,
acc
=
0.5
))
#arc
move4
=
dict
(
posI
=
dict
(
pos
=
dict
(
x
=
2
,
y
=
4
,
z
=
5
),
rot
=
dict
(
c
=
0
,
b
=
0
,
a
=
1
)),
posF
=
dict
(
pos
=
dict
(
x
=
0
,
y
=
1
,
z
=
0.5
),
rot
=
dict
(
c
=
0
,
b
=
1
,
a
=
0
)),
S_Ac
=
dict
(
speed
=
0.4
,
acc
=
1
))
#example of how to use variable length lists to make a "union"
moves
=
[
dict
(
move_lin
=
[
move1
],
move_ptp
=
[],
move_arc
=
[]),
dict
(
move_lin
=
[
move2
],
move_ptp
=
[],
move_arc
=
[]),
dict
(
move_lin
=
[],
move_ptp
=
[
move3
],
move_arc
=
[]),
dict
(
move_lin
=
[],
move_ptp
=
[],
move_arc
=
[
move4
]),]
client
.
CMD_MOVE
(
moves
)
client
.
shutdown
()
except
EOFError
:
print
"got EOF"
except
Exception
,
e
:
print
"got Exception"
print
e
traceback
.
print_exc
()
class
ROS_client
(
test_client
):
#GBSPose p
#float32 s
#float32 ac
#---
#float32 err1
#float32 err2
def
handle_gbs_linmove
(
self
,
req
):
ans
=
GBSLinMoveResponse
()
coordinate
=
req
.
p
.
pos
angle
=
req
.
p
.
ori
speed
=
req
.
s
acc
=
req
.
ac
try
:
res
=
self
.
CMD_MOVE_LIN
(
self
,
req
.
p
.
pos
,
req
.
p
.
ori
,
req
.
s
,
req
.
ac
)
ans
.
err1
=
0
# XXX
ans
.
err2
=
0
# XXX
except
:
ans
.
err1
=
GBS_ERROR
ans
.
err2
=
0
# XXX
return
ans
# GBSJointPose p
# float32 s
# float32 ac
# ---
# float32 err1
# float32 err2
def
handle_gbs_ptpmove
(
self
,
req
):
ans
=
GBSPTPMoveResponse
()
j
=
[
req
.
p
.
j1
,
req
.
p
.
j2
,
req
.
p
.
j3
,
req
.
p
.
j4
,
req
.
p
.
j5
,
req
.
p
.
j6
]
try
:
res
=
self
.
CMD_MOVE_PTP
(
j
,
req
.
s
,
req
.
ac
)
ans
.
err1
=
0
# XXX
ans
.
err2
=
0
# XXX
except
:
err1
=
GBS_ERROR
err2
=
0
# XXX
return
ans
#
# ---
#float32 err
#GBSJointPose p
def
handle_gbs_readcurjointpose
(
self
,
req
):
ans
=
GBSReadCurJointPoseResponse
()
try
:
res
=
self
.
CMD_READ_PTP
()
ans
.
err
=
0
# XXX
ans
.
p
.
j1
=
res
[
0
]
ans
.
p
.
j2
=
res
[
1
]
ans
.
p
.
j3
=
res
[
2
]
ans
.
p
.
j4
=
res
[
3
]
ans
.
p
.
j5
=
res
[
4
]
ans
.
p
.
j6
=
res
[
5
]
except
:
ans
.
err
=
GBS_ERROR
return
ans
#
# ---
# float32 err
def
handle_gbs_locktool
(
self
,
req
):
ans
=
GBSLockToolResponse
()
try
:
res
=
self
.
CMD_LCK_TL
()
ans
.
err
=
0
# XXX
except
:
ans
.
err
=
GBS_ERROR
return
ans
#
# ---
# float32 err
def
handle_gbs_unlocktool
(
self
,
req
):
ans
=
GBSUnlockToolResponse
()
try
:
res
=
self
.
CMD_UNLCK_TL
()
ans
.
err
=
0
# XXX
except
:
ans
.
err
=
GBS_ERROR
return
ans
# GBSPose pi
# GBSPose pf
# float32 s
# float32 ac
# ---
# float32 err1
# float32 err2
def
handle_gbs_circmove
(
self
,
req
):
posI
=
dict
(
pos
=
req
.
pi
.
pos
,
rot
=
req
.
pi
.
ori
)
posF
=
dict
(
pos
=
req
.
pf
.
pos
,
rot
=
req
.
pf
.
ori
)
ans
=
GBSCircMoveResponse
()
try
:
res
=
CMD_MOVE_ARC
(
posI
,
posF
,
req
.
s
,
req
.
ac
)
ans
.
err1
=
0
# XXX
ans
.
err2
=
0
# XXX
except
:
ans
.
err1
=
GBS_ERROR
ans
.
err2
=
GBS_ERROR
return
ans
# TODO
def
handle_gbs_combmove
(
self
,
req
):
raise
NotImplementedError
()
# int32 id
# GBSPose p
# float32 s
# float32 ac
# ---
# float32 err1
# float32 err2
def
handle_gbs_measuremove
(
self
,
req
):
ans
=
GBSMeasureMoveResponse
()
coord
=
req
.
p
.
pos
angle
=
req
.
p
.
ori
try
:
res
=
CMD_MOVE_MEASURE
(
req
.
id
,
coord
,
angle
,
req
.
s
,
req
.
ac
)
ans
.
err1
=
0
#XXX
ans
.
err2
=
0
#XXX
except
:
ans
.
err1
=
GBS_ERROR
ans
.
err2
=
GBS_ERROR
return
ans
# int32 id
# bool bstop
# float32 s
# float32 ac
# GBSPose p1
# GBSPose p2
# ---
# float32 err1
# float32 err2
def
handle_gbs_naillinmove
(
self
,
req
):
ans
=
GBSNailLinMoveResponse
()
start
=
dict
(
pos
=
req
.
p1
.
pos
,
rot
=
req
.
p1
.
ori
)
finish
=
dict
(
pos
=
req
.
p2
.
pos
,
rot
=
req
.
p2
.
ori
)
try
:
res
=
CMD_NAIL_LIN
(
req
.
id
,
req
.
bstop
,
start
,
finish
,
req
.
s
,
req
.
ac
)
ans
.
err1
=
0
#XXX
ans
.
err2
=
0
#XXX
except
:
ans
.
err1
=
GBS_ERROR
ans
.
err2
=
GBS_ERROR
return
ans
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment