Skip to content
GitLab
Projects
Groups
Snippets
Help
Loading...
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Open sidebar
Henrik Andreasson
harvest-automation
Commits
db4e2eb1
Commit
db4e2eb1
authored
Jul 23, 2020
by
Nicholas Shindler
Browse files
fixed errors with reader/writer
parent
3538da8b
Changes
9
Hide whitespace changes
Inline
Side-by-side
Showing
9 changed files
with
60 additions
and
40 deletions
+60
-40
hv_bridge/libs/Controllers/arm.py
hv_bridge/libs/Controllers/arm.py
+1
-1
hv_bridge/libs/Controllers/gripper.py
hv_bridge/libs/Controllers/gripper.py
+1
-1
hv_bridge/libs/Resources/client.py
hv_bridge/libs/Resources/client.py
+3
-4
hv_bridge/libs/Resources/listener.py
hv_bridge/libs/Resources/listener.py
+4
-5
hv_bridge/libs/Resources/reader.py
hv_bridge/libs/Resources/reader.py
+15
-2
hv_bridge/libs/Resources/writer.py
hv_bridge/libs/Resources/writer.py
+6
-2
hv_bridge/scripts/hv_client.py
hv_bridge/scripts/hv_client.py
+9
-4
hv_bridge/scripts/main_node.py
hv_bridge/scripts/main_node.py
+16
-16
hv_teleop/scripts/teleop_node.py
hv_teleop/scripts/teleop_node.py
+5
-5
No files found.
hv_bridge/libs/Controllers/arm.py
View file @
db4e2eb1
...
...
@@ -17,7 +17,7 @@ class ArmController(Writer):
def
__init__
(
self
,
hostname
):
# Create writer for this probe
# manip.armRequestedTargetPositionInPercent
Writer
.
__init__
(
self
,
[
"manip.armTargetPositionInPercent"
])
Writer
.
__init__
(
self
,
[
"manip.arm
Requested
TargetPositionInPercent"
])
# runs a subscriber for setting the arm position.
rospy
.
Subscriber
(
"/"
+
hostname
+
"/set_arm_pos"
,
Float32
,
self
.
callback
)
...
...
hv_bridge/libs/Controllers/gripper.py
View file @
db4e2eb1
...
...
@@ -17,7 +17,7 @@ class GripperController(Writer):
def
__init__
(
self
,
hostname
):
# Create writer for this probe
# manip.gripperRequestedTargetPositionInPercent
Writer
.
__init__
(
self
,
[
"manip.gripperTargetPositionInPercent"
])
Writer
.
__init__
(
self
,
[
"manip.gripper
Requested
TargetPositionInPercent"
])
# intializes the mp connection and runs a subscriber for setting the gripper position.
rospy
.
Subscriber
(
"/"
+
hostname
+
"/set_gripper_pos"
,
Float32
,
self
.
callback
)
...
...
hv_bridge/libs/Resources/client.py
View file @
db4e2eb1
...
...
@@ -94,9 +94,6 @@ class Client:
def
disconnect
(
self
):
try
:
# call e-stop to and all functionality before breaking connection
self
.
stop
()
# end the connection
self
.
stop_listener
()
self
.
s
.
close
()
...
...
@@ -166,7 +163,9 @@ class Client:
elif
type
(
probe
)
==
int
:
probe_id
=
probe
else
:
raise
ValueError
(
"probe must be int or string"
)
raise
ValueError
(
"probe must be int or string, probe is %s"
%
type
(
probe
)
)
return
probe_id
except
Exception
as
e
:
...
...
hv_bridge/libs/Resources/listener.py
View file @
db4e2eb1
...
...
@@ -42,11 +42,10 @@ class listen(threading.Thread):
if
not
self
.
client
.
do_async_message
(
t
,
l
,
v
):
self
.
q
.
put
((
t
,
l
,
v
))
try
:
if
self
.
callback
==
None
:
raise
NameError
(
"No Callback has been defined"
)
self
.
callback
(
t
,
v
)
except
NameError
:
pass
if
self
.
callback
!=
None
:
self
.
callback
(
t
,
v
)
except
Exception
as
e
:
rospy
.
logerr
(
e
)
# time.sleep(0.1)
except
Exception
as
e
:
rospy
.
logwarn
(
e
)
...
...
hv_bridge/libs/Resources/reader.py
View file @
db4e2eb1
...
...
@@ -2,6 +2,10 @@ class Reader:
# reader class for getting data from the robot and passing to ROS
def
__init__
(
self
,
p
):
# constructor for settings class
if
type
(
p
)
!=
tuple
and
type
(
p
)
!=
list
and
type
(
p
)
!=
str
:
raise
TypeError
(
"probes should be an itterable list/tupple or a string not %s"
%
type
(
p
)
)
self
.
pub
=
None
self
.
probes
=
p
self
.
queue
=
{}
...
...
@@ -15,6 +19,8 @@ class Reader:
def
add
(
self
,
name
,
val
):
# add data piece
try
:
if
type
(
self
.
probes
)
==
str
or
name
==
self
.
probes
:
self
.
queue
[
name
]
=
val
if
name
in
self
.
probes
:
self
.
queue
[
name
]
=
val
except
Exception
as
e
:
...
...
@@ -22,8 +28,15 @@ class Reader:
def
needs
(
self
,
name
):
# check if name is used by this class
return
True
if
name
in
self
.
probes
else
False
return
name
==
self
.
probes
if
type
(
self
.
probes
)
==
str
else
name
in
self
.
probes
def
get_probes
(
self
):
# return an array of all probes
return
self
.
probes
if
len
(
self
.
probes
)
>
1
else
self
.
probes
[
0
]
return
(
self
.
probes
if
len
(
self
.
probes
)
>
1
or
type
(
self
.
probes
)
==
str
else
self
.
probes
[
0
]
)
def
get_probe_list
(
self
):
return
self
.
probes
if
type
(
self
.
probes
)
!=
str
else
[
self
.
probes
]
hv_bridge/libs/Resources/writer.py
View file @
db4e2eb1
...
...
@@ -5,6 +5,10 @@ class Writer:
def
__init__
(
self
,
p
):
# constructor for settings class
if
type
(
p
)
!=
tuple
and
type
(
p
)
!=
list
and
type
(
p
)
!=
str
:
raise
TypeError
(
"probes should be an itterable list/tupple or a string not %s"
%
type
(
p
)
)
self
.
probes
=
p
def
get_msgs
(
self
):
...
...
@@ -19,10 +23,10 @@ class Writer:
def
callback
(
self
,
msg
):
# subscriber callback function
if
len
(
self
.
probes
)
==
1
:
if
type
(
self
.
probes
)
==
str
:
# if the message is a single item value pair
self
.
msgs
=
[(
self
.
probes
,
msg
)]
elif
len
(
msg
)
>
1
:
elif
type
(
msg
)
==
list
:
# handle an array of messages
# each message coresponds to a probe (1 to 1)
if
len
(
msg
)
!=
len
(
self
.
probes
):
...
...
hv_bridge/scripts/hv_client.py
View file @
db4e2eb1
...
...
@@ -111,14 +111,16 @@ class HVClient(Client):
try
:
# sets the start to FALSE. this 'stops' the robot.
# sets flag pulled to true
# TODO: switch codes for enums
Client
.
write_probes
(
self
,
[(
ESTOP
,
1
),
(
START
,
0
),
(
FLAG_PULL
,
1
)])
print
"virtual e-stop triggered"
time
.
sleep
(
1
)
Client
.
write_probes
(
self
,
[(
ESTOP
,
0
),
(
FLAG_PULL
,
0
)])
print
str
(
self
.
hostname
)
+
" has stopped all running processes"
except
Exception
as
e
:
rospy
.
logerr
(
e
)
raise
e
finally
:
Client
.
disconnect
(
self
)
def
enable_services
(
self
):
# take all probes that need to be listened for and enable them
...
...
@@ -128,7 +130,9 @@ class HVClient(Client):
if
len
(
b
)
>
1
else
tuple
(
a
.
union
(
b
[
0
]))
)
probes
=
get_probe_arr
(
set
([]),
map
((
lambda
a
:
set
(
a
.
probes
)),
self
.
lstnr
))
probes
=
get_probe_arr
(
set
([]),
map
((
lambda
a
:
set
(
a
.
get_probe_list
())),
self
.
lstnr
)
)
Client
.
enable_probes
(
self
,
probes
)
except
Exception
as
e
:
...
...
@@ -144,9 +148,8 @@ class HVClient(Client):
v
=
""
if
len
(
msgs
)
==
0
:
return
print
msgs
print
len
(
msgs
)
for
(
probe
,
value
)
in
msgs
:
print
(
probe
,
value
)
probe_id
=
Client
.
lookup_probe_id
(
self
,
probe
)
probe_type
=
Client
.
lookup_probe_type
(
self
,
probe_id
)
...
...
@@ -163,10 +166,12 @@ class HVClient(Client):
try
:
(
pname
,
ptype
)
=
Client
.
lookup_probe
(
self
,
t
)
val
=
Client
.
decode_probe_data
(
self
,
ptype
,
v
)
print
(
"name: "
+
pname
+
" val: "
+
val
)
[
serv
.
add
(
pname
,
val
)
for
serv
in
self
.
lstnr
]
except
Exception
as
e
:
rospy
.
logwarn
(
e
)
raise
e
def
publish
(
self
):
...
...
hv_bridge/scripts/main_node.py
View file @
db4e2eb1
...
...
@@ -94,7 +94,7 @@ if __name__ == "__main__":
# set the hostname based on the argument inputed by the user (default: hai-1095.local)
host_name
=
args
.
hostname
print
(
args
)
services
=
[]
if
args
.
all
:
# connect all sensors and controllers
...
...
@@ -112,7 +112,7 @@ if __name__ == "__main__":
]
else
:
if
args
.
sensor_flag
:
print
(
"using
all sensors
"
)
# add
all sensors
services
+=
[
ODOM_LSTN
,
IR_LSTN
,
...
...
@@ -122,7 +122,7 @@ if __name__ == "__main__":
ODOM_GYRO_LSTN
,
]
elif
args
.
sensors
!=
None
:
print
(
args
.
sensors
)
# add selected
sensors
if
1
in
args
.
sensors
:
services
.
append
(
LIDAR_LSTN
)
if
2
in
args
.
sensors
:
...
...
@@ -136,10 +136,10 @@ if __name__ == "__main__":
if
6
in
args
.
sensors
:
services
.
append
(
NEAREST_OBS
)
if
args
.
controller_flag
:
print
(
"using
all controllers
"
)
# add
all controllers
services
+=
[
DRIVE_CMD
,
GRIPPER_CMD
,
ARM_CMD
]
elif
args
.
controllers
!=
None
:
print
(
args
.
controllers
)
# add only the selected
controllers
if
3
in
args
.
sensors
:
services
.
append
(
DRIVE_CMD
)
if
2
in
args
.
controllers
:
...
...
@@ -147,7 +147,7 @@ if __name__ == "__main__":
if
1
in
args
.
controllers
:
services
.
append
(
ARM_CMD
)
if
"all"
in
args
.
services
:
print
(
"
services
!"
)
# add all
services
services
+=
[
FOLLOW_SRV
,
PICK_SRV
,
AVOID_SRV
]
elif
args
.
services
!=
[]:
if
"follow"
in
args
.
services
:
...
...
@@ -157,17 +157,16 @@ if __name__ == "__main__":
if
"avoid"
in
args
.
services
:
services
.
append
(
AVOID_SRV
)
print
(
"
c
onnecting to
{}"
).
format
(
host_name
)
rospy
.
loginfo
(
"
C
onnecting to
%s"
,
host_name
)
print
(
services
)
hv_bot
=
HVClient
(
host_name
,
services
)
# start the connection to the robot
hv_bot
.
start
()
time
.
sleep
(
1
)
time
.
sleep
(
2
)
# handle closing everything nicely when ros shutsdown
rospy
.
on_shutdown
(
lambda
:
hv_bot
.
stop
()
)
rospy
.
on_shutdown
(
hv_bot
.
stop
)
# enable the listener services
hv_bot
.
enable_services
()
...
...
@@ -182,7 +181,9 @@ if __name__ == "__main__":
# Start the ROS main loop
rate
=
rospy
.
Rate
(
hz
)
print
(
"starting publishing data from "
+
host_name
+
" at "
+
str
(
hz
)
+
"hz"
)
rospy
.
loginfo
(
"starting publishing data from "
+
host_name
+
" at "
+
str
(
hz
)
+
"hz"
)
while
not
rospy
.
is_shutdown
():
# write any messages from ROS to the robot
hv_bot
.
write
()
...
...
@@ -191,13 +192,12 @@ if __name__ == "__main__":
rate
.
sleep
()
except
rospy
.
ROSInterruptException
:
print
(
"ROS interupt called -- Ending Processes"
)
pass
rospy
.
logerr
(
"ROS interupt called -- Ending Processes"
)
except
Exception
as
e
:
# general error handling
err
=
traceback
.
format_exc
()
rospy
.
logerr
(
e
)
print
(
err
)
finally
:
# NOTE: this may not actually be called in most cases, and should be handled in the on shutdown function
# mp.disconnect()
print
(
"GOODBYE"
)
# last code to run before everything closes
rospy
.
loginfo
(
"GOODBYE"
)
hv_teleop/scripts/teleop_node.py
View file @
db4e2eb1
...
...
@@ -5,7 +5,7 @@ from __future__ import print_function
# import roslib; roslib.load_manifest('teleop_twist_keyboard')
import
rospy
from
geometry_msgs.msg
import
Twist
from
std_msgs.msg
import
Float64
from
std_msgs.msg
import
Float32
,
Float64
import
sys
,
os
,
select
,
termios
,
tty
,
math
...
...
@@ -155,10 +155,10 @@ if __name__ == "__main__":
"/"
+
resource_name
(
host_name
)
+
"/cmd_vel"
,
Twist
,
queue_size
=
1
)
pub_gripper
=
rospy
.
Publisher
(
"/"
+
resource_name
(
host_name
)
+
"/set_gripper_pos"
,
Float
64
,
queue_size
=
1
"/"
+
resource_name
(
host_name
)
+
"/set_gripper_pos"
,
Float
32
,
queue_size
=
1
)
pub_arm
=
rospy
.
Publisher
(
"/"
+
resource_name
(
host_name
)
+
"/set_arm_pos"
,
Float
64
,
queue_size
=
1
"/"
+
resource_name
(
host_name
)
+
"/set_arm_pos"
,
Float
32
,
queue_size
=
1
)
# init ros
...
...
@@ -185,8 +185,8 @@ if __name__ == "__main__":
print
(
pos
(
arm
,
gripper
))
# Create the msgs for arm and gripper
arm_msg
=
Float
64
()
gripper_msg
=
Float
64
()
arm_msg
=
Float
32
()
gripper_msg
=
Float
32
()
rate
=
rospy
.
Rate
(
100
)
# set update rate
# Start the ROS main loop
...
...
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