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
b64579f1
Commit
b64579f1
authored
Jul 06, 2020
by
Nicholas Shindler
Browse files
update to handle sensor
parent
228db3e3
Changes
5
Hide whitespace changes
Inline
Side-by-side
Showing
5 changed files
with
98 additions
and
42 deletions
+98
-42
hv_bridge/CMakeLists.txt
hv_bridge/CMakeLists.txt
+1
-0
hv_bridge/includes/Sensors/hub.py
hv_bridge/includes/Sensors/hub.py
+6
-3
hv_bridge/includes/Sensors/lidar.py
hv_bridge/includes/Sensors/lidar.py
+87
-39
hv_bridge/includes/Services/pick_target.py
hv_bridge/includes/Services/pick_target.py
+1
-0
hv_bridge/package.xml
hv_bridge/package.xml
+3
-0
No files found.
hv_bridge/CMakeLists.txt
View file @
b64579f1
...
...
@@ -76,6 +76,7 @@ generate_messages(
DEPENDENCIES
geometry_msgs
std_msgs
nav_msgs
)
################################################
...
...
hv_bridge/includes/Sensors/hub.py
View file @
b64579f1
...
...
@@ -27,6 +27,7 @@ class SensorHub():
"bound.rightRear.signalB"
:
0
,
"bound.leftRear.signalB"
:
0
,
"vis.sick.localScanData"
:
0
,
"vis.sick.globalScanData"
:
0
,
"vis.forwardObstacleDist"
:
0
,
"vis.forwardObstacle.x"
:
0
,
"vis.forwardObstacle.y"
:
0
,
...
...
@@ -75,7 +76,8 @@ class SensorHub():
# probe id 2866: vis.sick.globalScanData type tlv length 4372
# probe id 2867: vis.sick.localScanData type tlv length 4372
# probe id 2868: vis.sick.localScanData_EVERY_TICK type tlv length 4372
# self.lidar = LidarCamera("vis.sick.localScanData",3275,self.mp)
self
.
lidar
=
LidarCamera
(
"vis.sick.globalScanData"
,
2866
,
self
.
mp
)
# initialize the nearest obstacle detection of the preprocessed laser data
# codes give: x, y, and distance
# probe id 2928: vis.forwardObstacle.x type float length 4
...
...
@@ -110,6 +112,7 @@ class SensorHub():
self
.
odometeryGyro
=
Odometer
((
"loc.rawOdometryWGyro.x"
,
"loc.rawOdometryWGyro.y"
,
"loc.rawOdometryWGyro.h"
,
"loc.rawOdometryWGyro.v"
,
"loc.rawOdometryWGyro.w"
),(
2463
,
2464
,
2465
,
2466
,
2467
),
self
.
mp
)
#probe id 1100: manip.botGotPot type bool length 1
#probe id 3151: vis.PIGSensor.isPotInGripper type bool length 1
self
.
gripper
=
Gripper
(
"manip.botGotPot"
,
1100
,
self
.
mp
)
def
start
(
self
,
ros
,
hostname
):
...
...
@@ -141,7 +144,7 @@ class SensorHub():
self
.
rightRear
.
init
(
ros
,
hostname
)
# start the lidar camera
#
self.lidar.init(ros, hostname)
self
.
lidar
.
init
(
ros
,
hostname
)
self
.
forwardObstacleDist
.
init
(
ros
,
hostname
)
self
.
forwardObstacleX
.
init
(
ros
,
hostname
)
self
.
forwardObstacleY
.
init
(
ros
,
hostname
)
...
...
@@ -178,7 +181,7 @@ class SensorHub():
self
.
rightRear
.
run
(
self
.
queue
,
current_time
)
# run the lidar camera
#
self.lidar.run(self.queue)
self
.
lidar
.
run
(
self
.
queue
,
current_time
)
self
.
forwardObstacleDist
.
run
(
self
.
queue
)
self
.
forwardObstacleX
.
run
(
self
.
queue
)
self
.
forwardObstacleY
.
run
(
self
.
queue
)
...
...
hv_bridge/includes/Sensors/lidar.py
View file @
b64579f1
#!/usr/bin/env python
from
std_msgs.msg
import
Float64
from
std_msgs.msg
import
Header
from
std_msgs.msg
import
Int16MultiArray
from
nav_msgs.msg
import
PointCloud2
from
nav_msgs.msg
import
PointField
from
sensor_msgs.msg
import
PointField
from
sensor_msgs.msg
import
PointCloud2
import
sensor_msgs.point_cloud2
as
pc2
import
math
import
time
import
ros
import
sys
,
os
import
numpy
as
np
import
traceback
SCRIPTS_PATH
=
os
.
path
.
abspath
(
os
.
path
.
join
(
sys
.
path
[
0
]
,
"../../../resources"
))
sys
.
path
.
append
(
SCRIPTS_PATH
)
from
getter
import
Getter
...
...
@@ -18,7 +23,7 @@ class LidarCamera(Getter):
def
init
(
self
,
ros
,
hostname
):
self
.
hostname
=
hostname
# create publisher
self
.
pub
=
ros
.
Publisher
(
'/'
+
hostname
+
'/lidar/'
+
resource_name
(
self
.
name
),
Int16MultiArray
,
queue_size
=
1
)
self
.
pub
=
ros
.
Publisher
(
'/'
+
hostname
+
'/lidar/'
+
resource_name
(
self
.
name
),
PointCloud2
,
queue_size
=
1
)
# pass to parent to enble probes
#self.mp.write_probe(self.codes,1)
Getter
.
init
(
self
)
...
...
@@ -27,45 +32,88 @@ class LidarCamera(Getter):
## handles the publishing of data, should be run at an acceptable update interval
# get data from queue
try
:
data
=
q
[
self
.
name
]
if
(
type
(
data
)
!=
str
):
raise
Exception
(
"Data not in correct format!"
)
# get the data from the message
data
=
data
.
split
(
'v='
)[
-
1
]
if
(
data
==
''
):
raise
Exception
(
"lidar data is empty"
)
# create x and y arrays from the base16 string of data
strarr
=
data
.
split
(
':'
)
dat
=
[]
obj
=
[]
#f = lambda a : (x.append(int(a[0],16)),y.append(int(a[1],16)), f(a[2:])) if len(a[2:]) > 1 else (x.append(int(a[0],16)),y.append(int(a[1],16)))
#res = f(strarr)
for
i
in
range
(
len
(
strarr
)):
if
(
strarr
[
i
]
!=
''
):
dat
.
append
(
int
(
strarr
[
i
],
16
))
#y.append(int(strarr[i+1],16))
#obj.append([int(strarr[i],16), int(strarr[i+1],16), int(strarr[i+2],16)])
x
=
[]
y
=
[]
z
=
[]
for
i
in
range
(
0
,
len
(
dat
),
4
):
x
.
append
(
float
(
dat
[
i
])
+
float
(
dat
[
i
+
1
])
/
100.
)
y
.
append
(
float
(
dat
[
i
+
2
])
+
float
(
dat
[
i
+
3
])
/
100.
)
z
.
append
([
0.05
])
data
=
q
[
self
.
name
]
# get the data from the message
data
=
data
.
split
(
'v='
)[
-
1
]
if
(
data
==
''
):
return
# create x and y arrays from the base16 string of data
strarr
=
data
.
split
(
':'
)
x
=
[]
y
=
[]
(
lambda
a
:
(
x
.
append
(
int
(
a
[
0
],
16
)),
y
.
append
(
int
(
a
[
1
],
16
)),
fx
(
a
[
2
:]))
if
a
.
size
>
1
else
False
)(
strarr
)
# create pointcloud2 message
dataout
=
PointCloud2
()
# add timestamp based on ros time
msg
.
header
.
stamp
=
current_time
# add frame
msg
.
header
.
frame_id
=
self
.
hostname
# format message
N
=
len
(
x
)
xy
=
np
.
array
(
np
.
hstack
([
x
,
y
]),
dtype
=
np
.
float32
)
msg
.
height
=
1
msg
.
width
=
N
msg
.
fields
=
[
PointField
(
'x'
,
0
,
PointField
.
FLOAT32
,
1
),
PointField
(
'y'
,
4
,
PointField
.
FLOAT32
,
1
),
]
msg
.
is_bigendian
=
False
msg
.
point_step
=
8
msg
.
row_step
=
msg
.
point_step
*
N
msg
.
is_dense
=
True
;
msg
.
data
=
xy
.
tostring
()
x
=
np
.
array
(
x
)
y
=
np
.
array
(
y
)
z
=
np
.
array
(
z
)
# create pointcloud2 message
msg
=
PointCloud2
()
# add timestamp based on ros time
msg
.
header
.
stamp
=
current_time
# add frame
msg
.
header
.
frame_id
=
self
.
hostname
# format message
#header = Header()
#header.stamp = current_time
#header.frame_id = self.hostname
# publish data
Getter
.
run
(
self
,
dataout
)
#msg = pc2.create_cloud_xyz32(header,dat)
N
=
x
.
size
xyz
=
np
.
array
(
np
.
hstack
([
x
,
y
,
z
]),
dtype
=
np
.
float32
)
msg
.
height
=
1
msg
.
width
=
N
print
(
xyz
)
msg
.
fields
=
[
PointField
(
'x'
,
0
,
PointField
.
FLOAT32
,
1
),
PointField
(
'y'
,
4
,
PointField
.
FLOAT32
,
1
),
PointField
(
'z'
,
8
,
PointField
.
FLOAT32
,
1
),
]
#msg.is_bigendian = False
msg
.
point_step
=
12
msg
.
row_step
=
msg
.
point_step
*
N
msg
.
is_dense
=
True
;
msg
.
data
=
xyz
.
tostring
()
# publish data
Getter
.
run
(
self
,
msg
)
except
Exception
as
e
:
# well this is sad
err
=
traceback
.
format_exc
()
print
(
err
)
# create pointcloud2 message
msg
=
PointCloud2
()
msg
.
header
.
stamp
=
current_time
msg
.
header
.
frame_id
=
self
.
hostname
# publish data
Getter
.
run
(
self
,
msg
)
# enable_probes((3275, 2876,2868,2867,2866,2930))
# enabling 3275 will collect data from laser sensor
...
...
hv_bridge/includes/Services/pick_target.py
View file @
b64579f1
...
...
@@ -16,6 +16,7 @@ SET_Y_LOC_CODE = 2439
SET_H_LOC_CODE
=
2440
PSEUDO_START_CODE
=
1125
BOT_GOT_POT_CODE
=
1100
POT_IN_GRIPPER
=
3151
class
PickTargetService
():
...
...
hv_bridge/package.xml
View file @
b64579f1
...
...
@@ -52,6 +52,7 @@
<buildtool_depend>
catkin
</buildtool_depend>
<build_depend>
geometry_msgs
</build_depend>
<build_depend>
nav_msgs
</build_depend>
<build_depend>
sensor_msgs
</build_depend>
<build_depend>
rospy
</build_depend>
<build_depend>
std_msgs
</build_depend>
<build_depend>
tf
</build_depend>
...
...
@@ -59,11 +60,13 @@
<build_export_depend>
rospy
</build_export_depend>
<build_export_depend>
nav_msgs
</build_export_depend>
<build_export_depend>
std_msgs
</build_export_depend>
<build_export_depend>
sensor_msgs
</build_export_depend>
<build_export_depend>
tf
</build_export_depend>
<exec_depend>
geometry_msgs
</exec_depend>
<exec_depend>
rospy
</exec_depend>
<exec_depend>
nav_msgs
</exec_depend>
<exec_depend>
std_msgs
</exec_depend>
<exec_depend>
sensor_msgs
</exec_depend>
<exec_depend>
tf
</exec_depend>
...
...
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