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
b302efa2
Commit
b302efa2
authored
Jul 22, 2020
by
Nicholas Shindler
Browse files
add input arguments to main class
parent
41dd7906
Changes
11
Hide whitespace changes
Inline
Side-by-side
Showing
11 changed files
with
205 additions
and
112 deletions
+205
-112
hv_bridge/libs/Resources/__init__.py
hv_bridge/libs/Resources/__init__.py
+6
-0
hv_bridge/libs/Resources/client.py
hv_bridge/libs/Resources/client.py
+2
-3
hv_bridge/libs/Resources/config.py
hv_bridge/libs/Resources/config.py
+16
-0
hv_bridge/libs/Resources/listener.py
hv_bridge/libs/Resources/listener.py
+5
-3
hv_bridge/libs/Resources/reader.py
hv_bridge/libs/Resources/reader.py
+3
-3
hv_bridge/libs/Resources/writer.py
hv_bridge/libs/Resources/writer.py
+1
-1
hv_bridge/libs/Sensors/hub.py
hv_bridge/libs/Sensors/hub.py
+4
-4
hv_bridge/libs/Sensors/lidar.py
hv_bridge/libs/Sensors/lidar.py
+15
-13
hv_bridge/scripts/hv_client.py
hv_bridge/scripts/hv_client.py
+11
-1
hv_bridge/scripts/main_node.py
hv_bridge/scripts/main_node.py
+141
-83
hv_launch/lauch/teleop.launch
hv_launch/lauch/teleop.launch
+1
-1
No files found.
hv_bridge/libs/Resources/__init__.py
View file @
b302efa2
...
...
@@ -4,6 +4,12 @@
# include all the utility scripts
###
from
client
import
Client
from
reader
import
Reader
from
writer
import
Writer
from
config
import
Serv
from
utils
import
*
from
getter
import
*
from
setter
import
*
...
...
hv_bridge/libs/Resources/client.py
View file @
b302efa2
...
...
@@ -100,7 +100,6 @@ class Client:
print
"Client Disconnet Failed"
raise
e
## ----- Probe Methods ----- ##
def
discover_probes
(
self
):
...
...
@@ -205,7 +204,7 @@ class Client:
return
data
except
:
raise
RuntimeError
(
"struct unpack failed"
)
def
start_probes
(
self
):
# start capturing information from a probe
self
.
capturing
=
True
...
...
@@ -215,7 +214,7 @@ class Client:
# stop capturing information from a probe
self
.
send_message
(
make_tlv
(
MP_TLV_STOP_PROBES
))
self
.
capturing
=
False
def
write_probe
(
self
,
probe
,
value
):
# write a vale to a probe
probe_id
=
self
.
lookup_probe_id
(
probe
)
...
...
hv_bridge/libs/Resources/config.py
View file @
b302efa2
import
enum.Enum
class
Serv
(
Enum
):
DRIVE_CMD
=
1
GRIPPER_CMD
=
2
ARM_CMD
=
3
ODOM_CMD
=
4
ODOM_LSTN
=
5
IR_LSTN
=
6
LIDAR_LSTN
=
7
GRIPPER_LSTN
=
8
NEAREST_OBS
=
9
ODOM_GYRO_LSTN
=
10
FOLLOW_SRV
=
11
PICK_SRV
=
12
AVOID_SRV
=
13
hv_bridge/libs/Resources/listener.py
View file @
b302efa2
...
...
@@ -6,7 +6,7 @@ class listen(threading.Thread):
self
.
mp
=
mp
self
.
callback
=
None
self
.
setDaemon
(
True
)
def
addMsgCallback
(
callbackFcn
):
# adds a function that is called whenever a message is recived by the queue
self
.
callback
=
callbackFcn
...
...
@@ -17,7 +17,9 @@ class listen(threading.Thread):
while
True
:
# Wait for incoming data from the RCP or for the listener
# pipe to close, indicating that we are
(
r
,
w
,
x
)
=
select
.
select
((
self
.
client
.
s
,
self
.
client
.
listener_pipe
[
0
]),
(),
())
(
r
,
w
,
x
)
=
select
.
select
(
(
self
.
client
.
s
,
self
.
client
.
listener_pipe
[
0
]),
(),
()
)
if
self
.
client
.
listener_pipe
[
0
]
in
r
:
# listener pipe signaled the thread to exit
os
.
close
(
self
.
client
.
listener_pipe
[
0
])
...
...
@@ -29,7 +31,7 @@ class listen(threading.Thread):
try
:
if
self
.
callback
==
None
:
raise
NameError
(
"No Callback has been defined"
)
self
.
callback
(
t
,
v
)
self
.
callback
(
t
,
v
)
except
NameError
:
pass
# time.sleep(0.1)
...
...
hv_bridge/libs/Resources/reader.py
View file @
b302efa2
class
Reader
()
:
class
Reader
:
# reader class for getting data from the robot and passing to ROS
def
__init__
(
self
,
name
,
p
):
# constructor for settings class
...
...
@@ -11,14 +11,14 @@ class Reader():
def
run
(
self
,
data
):
## Publish data
self
.
pub
.
publish
(
data
)
def
add
(
name
,
val
):
# add data piece
try
:
self
.
queue
[
name
]
=
val
except
Exception
as
e
:
raise
e
def
needs
(
name
):
# check if name is used by this class
return
True
if
name
in
self
.
probes
else
False
...
...
hv_bridge/libs/Resources/writer.py
View file @
b302efa2
class
Reader
()
:
class
Writer
:
# reader class for getting data from the robot and passing to ROS
def
__init__
(
self
,
p
):
# constructor for settings class
...
...
hv_bridge/libs/Sensors/hub.py
View file @
b302efa2
...
...
@@ -101,11 +101,11 @@ class SensorHub:
# probe id 3199: vis.lineSensor.line.now.pt.y type float length 4
# probe id 3182: vis.lineSensor.cand.local.pt.x type float length 4
# probe id 3183: vis.lineSensor.cand.local.pt.y type float length 4
#self.sick = SickLineSensor(
#
self.sick = SickLineSensor(
# ("vis.lineSensor.line.now.pt.x", "vis.lineSensor.line.now.pt.y"),
# (3198, 3199),
# self.mp,
#)
#
)
# initialize the nearest obstacle detection of the preprocessed laser data
# codes give: x, y, and distance
...
...
@@ -200,7 +200,7 @@ class SensorHub:
# start the lidar camera
self
.
lidar
.
init
(
ros
,
hostname
)
#self.sick.init(ros, hostname)
#
self.sick.init(ros, hostname)
self
.
forwardObs
.
init
(
ros
,
hostname
)
# start the odometery
...
...
@@ -236,7 +236,7 @@ class SensorHub:
# run the lidar camera
self
.
lidar
.
run
(
self
.
queue
,
current_time
)
#self.sick.run(self.queue, current_time)
#
self.sick.run(self.queue, current_time)
self
.
forwardObs
.
run
(
self
.
queue
,
current_time
)
# run the odometry
...
...
hv_bridge/libs/Sensors/lidar.py
View file @
b302efa2
...
...
@@ -44,7 +44,7 @@ class LidarSensor(Getter):
try
:
data
=
q
[
self
.
name
]
#print data
#
print data
if
type
(
data
)
!=
str
:
raise
Exception
(
"Data not in correct format!"
)
# get the data from the message
...
...
@@ -58,25 +58,27 @@ class LidarSensor(Getter):
for
elem
in
strarr
:
if
elem
!=
""
:
datStr
+=
chr
(
int
(
elem
,
16
))
dat
=
[
struct
.
unpack
(
"<f"
,
datStr
[
i
:
i
+
4
])
for
i
in
range
(
0
,
len
(
datStr
),
4
)]
dat
=
[
struct
.
unpack
(
"<f"
,
datStr
[
i
:
i
+
4
])
for
i
in
range
(
0
,
len
(
datStr
),
4
)
]
#elem_arr = np.zeros((361,3),dtype=float)
#
elem_arr = np.zeros((361,3),dtype=float)
elem_arr
=
[]
j
=
0
for
i
in
range
(
0
,
len
(
dat
),
3
):
if
len
(
dat
[
i
:])
<
3
:
break
#if j > 360:
#
if j > 360:
# break
elem_arr
.
append
([
dat
[
i
],
dat
[
i
+
1
],
0.05
])
elem_arr
.
append
([
dat
[
i
],
dat
[
i
+
1
],
0.05
])
z
=
0.0
#print "x = %f, y = %f" % (x,y)
#elem_arr[j,0] = dat[i]
#elem_arr[j,1] = dat[i+1]
#j = j + 1
#
print "x = %f, y = %f" % (x,y)
#
elem_arr[j,0] = dat[i]
#
elem_arr[j,1] = dat[i+1]
#
j = j + 1
elem_arr
=
np
.
array
(
elem_arr
)
header
=
Header
()
header
.
stamp
=
current_time
header
.
frame_id
=
self
.
hostname
...
...
@@ -89,7 +91,7 @@ class LidarSensor(Getter):
except
Exception
as
e
:
# well this is sad
err
=
traceback
.
format_exc
()
print
(
err
)
print
(
err
)
# create pointcloud2 message
msg
=
PointCloud2
()
msg
.
header
.
stamp
=
current_time
...
...
@@ -153,7 +155,7 @@ class LidarNO(Getter):
except
Exception
as
e
:
# well this is sad
err
=
traceback
.
format_exc
()
print
(
err
)
print
(
err
)
class
SickLineSensor
(
Getter
):
...
...
@@ -217,4 +219,4 @@ class SickLineSensor(Getter):
except
Exception
as
e
:
# well this is sad
err
=
traceback
.
format_exc
()
print
(
err
)
print
(
err
)
hv_bridge/scripts/hv_client.py
View file @
b302efa2
...
...
@@ -58,6 +58,16 @@ class HVClient(Client):
if
ODOM_GYRO_LSTN
in
services
:
# enable odometry with gryro correction listener
self
.
lstnr
.
append
(
OdometryGyroListener
())
if
FOLLOW_SRV
in
services
:
# enable odometry with gryro correction listener
self
.
srvs
.
append
(
FollowService
())
if
PICK_SRV
in
services
:
# enable odometry with gryro correction listener
self
.
svrs
.
append
(
PickTargetService
())
if
AVOID_SRV
in
services
:
# enable odometry with gryro correction listener
self
.
srvs
.
append
(
AvoidService
())
Client
(
self
)
except
Exception
as
e
:
...
...
@@ -107,7 +117,7 @@ class HVClient(Client):
Client
.
enable_probes
(
self
,
probes
)
def
write
(
self
,
msgs
):
def
write
(
self
):
# gather all messages from controll class instances and write to the robot
try
:
get_msg_arr
=
lambda
a
,
b
:
get_msg_arr
(
a
+
b
[
0
],
b
[
1
:])
if
len
(
b
)
>
1
else
a
+
b
[
0
])
...
...
hv_bridge/scripts/main_node.py
View file @
b302efa2
#!/usr/bin/env python
import
rospy
import
argparse
import
traceback
import
sys
,
os
,
time
# add local python scripts to the path so that they can be iported
...
...
@@ -22,106 +24,161 @@ if __name__ == "__main__":
try
:
# Init the connection with the ROS system
rospy
.
init_node
(
"hai_main_node"
,
anonymous
=
True
)
# TODO: add rospy.on_shutdown(callback)
# Init the argument parser
parser
=
argparse
.
ArgumentParser
(
description
=
"ROS Connection Application for comunication and control of the HV Robots"
)
# Add the arguments to handle setting various sensor and controller connections
parser
.
add_argument
(
"hostname"
,
metavar
=
"hostname"
,
type
=
str
,
default
=
"hai-1095.local"
,
nargs
=
"?"
,
help
=
"The name of the robot, generally of the form hai-####.local (default: hai-1095.local"
,
)
parser
.
add_argument
(
"-a"
,
"--all"
,
action
=
"store_true"
,
dest
=
"all"
,
help
=
"Use all the sensors and controllers"
,
)
parser
.
add_argument
(
"--all-controllers"
,
action
=
"store_true"
,
dest
=
"controller_flag"
,
help
=
"Enable all controller classes"
,
)
parser
.
add_argument
(
"-c"
,
"--controllers"
,
type
=
int
,
nargs
=
"+"
,
dest
=
"controllers"
,
choices
=
[
1
,
2
,
3
],
help
=
"Set the options for which contollers to use (choices: 1 - arm, 2 - gripper, 3 - drive)"
,
)
parser
.
add_argument
(
"--all-sensors"
,
action
=
"store_true"
,
dest
=
"sensor_flag"
,
help
=
"Enable all sensor classes"
,
)
parser
.
add_argument
(
"-s"
,
"--sensors"
,
type
=
int
,
nargs
=
"+"
,
dest
=
"sensors"
,
choices
=
[
1
,
2
,
3
,
4
,
5
,
6
],
help
=
"Set the options for which sensors to use (choices: 1 - lidar, 2 - gripper, 3 - ir, 4 - odometry, 5 - odometry w/ gyro, 6 - nearest object)"
,
)
parser
.
add_argument
(
"--services"
,
type
=
str
,
nargs
=
"*"
,
dest
=
"services"
,
choices
=
[
None
,
"all"
,
"follow"
,
"pick"
,
"avoid"
],
default
=
"all"
,
help
=
"Set the services to make available"
,
)
# if present, set the hostname based on the argument inputed by the user
if
len
(
sys
.
argv
)
<
2
:
host_name
=
"hai-1095.local"
args
=
parser
.
parse_args
()
# 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
print
(
args
.
all
)
services
=
[
DRIVE_CMD
,
GRIPPER_CMD
,
ARM_CMD
,
ODOM_LSTN
,
IR_LSTN
,
LIDAR_LSTN
,
GRIPPER_LSTN
,
NEAREST_OBS
,
ODOM_GYRO_LSTN
,
]
else
:
host_name
=
sys
.
argv
[
1
]
print
(
"connected to {}"
).
format
(
host_name
)
# Create the mindprobe instance
mp
=
Mindprobe
()
mp
.
init
(
host_name
)
# wait while the mp connection is fully established
while
len
(
mp
.
return_probes
())
<
3000
:
time
.
sleep
(
1
)
### Create all the Controllers ###
drive
=
DriveController
(
mp
)
arm
=
ArmController
(
mp
)
gripper
=
GripperController
(
mp
)
### -------------------------- ###
### Create Sensor Hub ###
hub
=
SensorHub
(
mp
)
### ----------------- ###
if
args
.
sensor_flag
:
print
(
"using all sensors"
)
services
+=
[
ODOM_LSTN
,
IR_LSTN
,
LIDAR_LSTN
,
GRIPPER_LSTN
,
NEAREST_OBS
,
ODOM_GYRO_LSTN
,
]
elif
args
.
sensors
!=
None
:
print
(
args
.
sensors
)
if
1
in
args
.
sensors
:
services
.
append
(
LIDAR_LSTN
)
if
2
in
args
.
sensors
:
services
.
append
(
GRIPPER_LSTN
)
if
3
in
args
.
sensors
:
services
.
append
(
IR_LSTN
)
if
4
in
args
.
sensors
:
services
.
append
(
ODOM_LSTN
)
if
5
in
args
.
sensors
:
services
.
append
(
ODOM_GYRO_LSTN
)
if
6
in
args
.
sensors
:
services
.
append
(
NEAREST_OBS
)
if
args
.
controller_flag
:
print
(
"using all controllers"
)
services
+=
[
DRIVE_CMD
,
GRIPPER_CMD
,
ARM_CMD
]
elif
args
.
controllers
!=
None
:
print
(
args
.
controllers
)
if
3
in
args
.
sensors
:
services
.
append
(
DRIVE_CMD
)
if
2
in
args
.
controllers
:
services
.
append
(
GRIPPER_CMD
)
if
1
in
args
.
controllers
:
services
.
append
(
ARM_CMD
)
if
"all"
in
args
.
services
:
print
(
"services!"
)
services
+=
[
FOLLOW_SRV
,
PICK_SRV
,
AVOID_SRV
]
elif
args
.
services
!=
[]:
if
"follow"
in
args
.
services
:
services
.
append
(
FOLLOW_SRV
)
if
"pick"
in
args
.
services
:
services
.
append
(
PICK_SRV
)
if
"avoid"
in
args
.
services
:
services
.
append
(
AVOID_SRV
)
print
(
"connecting to {}"
).
format
(
host_name
)
hv_bot
=
HVClient
(
services
)
# start the connection to the robot
hv_bot
.
start
()
time
.
sleep
(
1
)
### Drive Controller ###
drive
.
run
(
rospy
,
resource_name
(
host_name
))
print
(
"DRIVE control running"
)
### --------------- ###
### Arm Controller ###
arm
.
run
(
rospy
,
resource_name
(
host_name
))
print
(
"ARM control running"
)
### --------------- ###
### Gripper Controller ###
gripper
.
run
(
rospy
,
resource_name
(
host_name
))
print
(
"GRIPPER control running"
)
### --------------- ###
### Sensor Hub ###
hub
.
start
(
rospy
,
resource_name
(
host_name
))
print
(
"sensor HUB running"
)
### --------------- ###
# handle closing everything nicely when ros shutsdown
rospy
.
on_shutdown
(
lambda
:
hv_bot
.
stop
())
# enable the listener services
hv_bot
.
enable_services
()
time
.
sleep
(
1
)
### Start Mindprobe Connection ###
# enables probes and starts mode to allow listening
mp
.
start
()
### -------------------------- ###
### Start Services ###
follow
=
FollowService
(
mp
,
rospy
)
# allows the follow me behavior to be toggled on.
pick
=
PickTargetService
(
mp
,
rospy
)
# allows the setting an x-y coord to collect a pot from.
avoid
=
AvoidService
(
mp
,
rospy
)
# allows the avoid collision behavior to be toggled on.
### -------------- ###
time
.
sleep
(
3
)
# handle closing everything nicely when ros shutsdown
rospy
.
on_shutdown
(
lambda
:
follow
.
stop
()
&
pick
.
stop
()
&
avoid
.
stop
()
&
mp
.
disconnect
()
)
# set rate based on the mindprobe refresh rate.
# default to 100. (mp runs at 200)
hz
=
100
if
mp
.
hz
:
hz
=
mp
.
hz
# timer_count_1 = 0
# timer_count_2 = 0
# Start the ROS main loop
rate
=
rospy
.
Rate
(
hz
)
print
(
"starting publishing data from "
+
host_name
+
" at "
+
str
(
hz
)
+
"hz"
)
while
not
rospy
.
is_shutdown
():
# if(timer_count_1 > 10):
# # run the sensor data collection
# hub.listen()
# timer_count_1 = 0
# if(timer_count_2 > 100):
# hub.scan()
# timer_count_2 = 0
# if(timer_count_2 == 1):
# mp.write_probe(3859,1)
# timer_count_1 = timer_count_1 + 1
# timer_count_2 = timer_count_2 + 1
hub
.
listen
(
rospy
.
get_rostime
())
# write any messages from ROS to the robot
hv_bot
.
write
()
rate
.
sleep
()
...
...
@@ -130,7 +187,8 @@ if __name__ == "__main__":
pass
except
Exception
as
e
:
# general error handling
print
(
e
)
err
=
traceback
.
format_exc
()
print
(
err
)
finally
:
# NOTE: this may not actually be called in most cases, and should be handled in the on shutdown function
# mp.disconnect()
...
...
hv_launch/lauch/teleop.launch
View file @
b302efa2
<launch>
<node name="main_node.py" pkg="hv_bridge" type="bridge" args="--controllers" />
<node name="main_node.py" pkg="hv_bridge" type="bridge" args="--
all-
controllers" />
<node name="teleop_node.py" pkg="hv_teleop" type="talker" />
</launch>
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