Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add support from Hokuyo lidar #1186

Open
wants to merge 3 commits into
base: main
Choose a base branch
from
Open

Add support from Hokuyo lidar #1186

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
05089ae
[UNTESTED] Add support for Hokuyo lidar in donkeycar/parts/lidar.py a…
mgagvani Jun 1, 2024
e526ce7
lidar viz as as camera, fixes
mgagvani Jun 2, 2024
435d3fb
fixes addressing @Ezward review
mgagvani Jul 1, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
50 changes: 46 additions & 4 deletions donkeycar/parts/lidar.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -389,6 +389,41 @@ def shutdown(self):
self.lidar.StopScanning()
self.lidar.Disconnect()

class HokuyoLidar(object):
'''
Class for ethernet-based Hokuyo lidars (e.g. UST-10LX in F1Tenth)
'''
def __init__(self, max_dist):
'''
max_dist: maximum distance in mm (e.g. 20,000 for UST-20LX)
'''
from hokuyolx import HokuyoLX
self.laser = HokuyoLX()
self.DMAX = max_dist

def poll(self):
timestamp, scan = self.laser.get_filtered_dist(dmax=self.DMAX)

# shape (n, 2) --> list of (theta, r, _, _, _)
angles, distances = scan[:,0], scan[:,1]
angles = np.rad2deg(angles)
filler = np.zeros_like(angles) # for LidarPlot
self.scan = np.stack((distances, angles, filler, filler, filler), axis=-1)

def update(self):
self.poll()
time.sleep(0) # copied from RPLidar2 (release to other threads)

def run_threaded(self):
return self.scan

def run(self):
self.poll()
return self.scan

def shutdown(self):
self.laser.close()


class LidarPlot(object):
'''
Expand Down Expand Up @@ -643,7 +678,7 @@ def plot_polar_angle(draw_context, bounds, color, theta,
class LidarPlot2(object):
'''
takes the lidar measurements as a list of (distance, angle) tuples
and plots them to a PIL image which it outputs
and plots them to a CV2 (numpy) image which it outputs

resolution: dimensions of image in pixels as tuple (width, height)
plot_type: PLOT_TYPE_CIRC or PLOT_TYPE_LINE
Expand Down Expand Up @@ -702,13 +737,20 @@ def run(self, measurements):
plot_polar_angle(draw, bounds, self.border_color, 0,
self.angle_direction, self.rotate_plot)

# data points
if measurements:
plot_polar_points(
draw, bounds, self.mark_fn, self.point_color, self.mark_px,
[(distance, angle) for distance, angle, _, _, _ in measurements],
self.max_distance, self.angle_direction, self.rotate_plot)

# data points
plot_polar_points(
draw, bounds, self.mark_fn, self.point_color, self.mark_px,
[(distance, angle) for distance, angle, _, _, _ in measurements],
self.max_distance, self.angle_direction, self.rotate_plot)

return self.frame
return np.asarray(self.frame) # convert to image

def shutdown(self):
pass
Expand Down Expand Up @@ -893,8 +935,8 @@ def convert_from_image_to_cv2(img: Image) -> np.ndarray:
img = plotter.run(measurements)

# show the image in the window
cv2img = convert_from_image_to_cv2(img)
cv2.imshow("lidar", cv2img)
# cv2img = convert_from_image_to_cv2(img)
cv2.imshow("lidar", img)

if not args.threaded:
key = cv2.waitKey(1) & 0xFF
Expand Down
6 changes: 4 additions & 2 deletions donkeycar/templates/cfg_complete.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@
MAX_LOOPS = None # the vehicle loop can abort after this many iterations, when given a positive integer.

#CAMERA
CAMERA_TYPE = "PICAM" # (PICAM|WEBCAM|CVCAM|CSIC|V4L|D435|MOCK|IMAGE_LIST)
CAMERA_TYPE = "PICAM" # (PICAM|WEBCAM|CVCAM|CSIC|V4L|D435|OAKD|MOCK|IMAGE_LIST|LIDAR_PLOT)
IMAGE_W = 160
IMAGE_H = 120
IMAGE_DEPTH = 3 # default RGB=3, make 1 for mono
Expand Down Expand Up @@ -349,9 +349,11 @@

# #LIDAR
USE_LIDAR = False
LIDAR_TYPE = 'RP' #(RP|YD)
LIDAR_TYPE = 'RP' #(RP|YD|HOKUYO)
LIDAR_LOWER_LIMIT = 90 # angles that will be recorded. Use this to block out obstructed areas on your car, or looking backwards. Note that for the RP A1M8 Lidar, "0" is in the direction of the motor
LIDAR_UPPER_LIMIT = 270
LIDAR_MAX_DIST = 10_000 # Maximum distance for LiDAR. Measured in mm
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

These values need to have defaults based on the chose lidar type. RP/YD would be 0 for angle and I'm not sure for distance.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Do you mean these should be set to zero for the user to put in themselves? I wanted to give this example to show that the back half of scans should likely be discarded in most set-ups. As for the max distance, I think this is reasonable for the entry level RP Lidar/ YD Lidar. If someone buys a fancier model then I would assume they would also know how to change this value to reflect the capabilities of their lidar.

LIDAR_ANGLE_OFFSET = 135 # The zero point of the LiDAR is rotated this much in degrees on LidarPlot

# TFMINI
HAVE_TFMINI = False
Expand Down
25 changes: 24 additions & 1 deletion donkeycar/templates/complete.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -106,13 +106,18 @@ def drive(cfg, model_path=None, use_joystick=False, model_type=None,

# add lidar
if cfg.USE_LIDAR:
from donkeycar.parts.lidar import RPLidar
if cfg.LIDAR_TYPE == 'RP':
from donkeycar.parts.lidar import RPLidar
print("adding RP lidar part")
lidar = RPLidar(lower_limit = cfg.LIDAR_LOWER_LIMIT, upper_limit = cfg.LIDAR_UPPER_LIMIT)
V.add(lidar, inputs=[],outputs=['lidar/dist_array'], threaded=True)
if cfg.LIDAR_TYPE == 'YD':
print("YD Lidar not yet supported")
if cfg.LIDAR_TYPE == "HOKUYO":
from donkeycar.parts.lidar import HokuyoLidar
print("adding Hokuyo lidar part")
lidar = HokuyoLidar(max_dist = cfg.LIDAR_MAX_DIST)
V.add(lidar, inputs=[], outputs=['lidar/dist_scan'], threaded=False)

if cfg.HAVE_TFMINI:
from donkeycar.parts.tfmini import TFMini
Expand Down Expand Up @@ -820,6 +825,9 @@ def get_camera(cfg):
elif cfg.CAMERA_TYPE == "MOCK":
from donkeycar.parts.camera import MockCamera
cam = MockCamera(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH)
elif cfg.CAMERA_TYPE == "LIDAR_PLOT":
from donkeycar.parts.lidar import LidarPlot2
cam = LidarPlot2(resolution=(cfg.IMAGE_H, cfg.IMAGE_W))
else:
raise(Exception("Unkown camera type: %s" % cfg.CAMERA_TYPE))
return cam
Expand Down Expand Up @@ -872,7 +880,22 @@ def add_camera(V, cfg, camera_type):
outputs=['cam/image_array', 'cam/depth_array',
'imu/acl_x', 'imu/acl_y', 'imu/acl_z',
'imu/gyr_x', 'imu/gyr_y', 'imu/gyr_z'],
threaded=False)


elif cfg.CAMERA_TYPE == "LIDAR_PLOT":
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is very interesting. So the idea here is that the lidar plot would be used as the input to the CNN? Separate from the code concerns, have you tried this? Does it produce a reasonable autopilot?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I was thinking more along the lines of being able to see what the lidar sees. I haven't tried to train an autopilot with it although that does sound like an interesting idea.

from donkeycar.parts.lidar import LidarPlot2
cam = LidarPlot2(
resolution=(cfg.IMAGE_W, cfg.IMAGE_H),
rotate_plot=cfg.LIDAR_ANGLE_OFFSET,
max_dist=cfg.LIDAR_MAX_DIST,
plot_type=LidarPlot2.PLOT_TYPE_CIRCLE,
mark_px=1
)
V.add(cam, inputs=['lidar/dist_scan'],
outputs=['cam/image_array'],
threaded=True)

else:
inputs = []
outputs = ['cam/image_array']
Expand Down
Loading