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 1 commit
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
17 changes: 11 additions & 6 deletions donkeycar/parts/lidar.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -402,17 +402,17 @@ def __init__(self, max_dist):
self.DMAX = max_dist

def poll(self):
# scan is list of (theta, r) - e.g. shape of 1000, 2
timestamp, scan = self.laser.get_filtered_dist(dmax=self.DMAX)

# flip so it's (r, theta) --> compatible with LidarPlot
distances = scan[,:1]
angles = scan[,:0]
self.scan = np.hstack(distances, angles)
# 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
time.sleep(0) # copied from RPLidar2 (release to other threads)

def run_threaded(self):
return self.scan
Expand Down Expand Up @@ -737,6 +737,11 @@ def run(self, measurements):
plot_polar_angle(draw, bounds, self.border_color, 0,
self.angle_direction, self.rotate_plot)

# handle no data yet
if measurements is None:
mgagvani marked this conversation as resolved.
Show resolved Hide resolved
print("bruh")
mgagvani marked this conversation as resolved.
Show resolved Hide resolved
return np.asarray(self.frame)

# data points
plot_polar_points(
draw, bounds, self.mark_fn, self.point_color, self.mark_px,
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
28 changes: 26 additions & 2 deletions donkeycar/templates/complete.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -116,8 +116,8 @@ def drive(cfg, model_path=None, use_joystick=False, model_type=None,
if cfg.LIDAR_TYPE == "HOKUYO":
from donkeycar.parts.lidar import HokuyoLidar
print("adding Hokuyo lidar part")
lidar = HokuyoLidar(max_dist = 10_000)
V.add(lidar, inputs=[], outputs=['lidar/dist_scan'], threaded=True)
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 @@ -880,7 +880,31 @@ 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 == "OAKD":
mgagvani marked this conversation as resolved.
Show resolved Hide resolved
from donkeycar.parts.oak_d import OakD
cam = OakD(
enable_rgb=cfg.OAKD_RGB,
enable_depth=cfg.OAKD_DEPTH,
device_id=cfg.OAKD_ID)
V.add(cam, inputs=[],
outputs=['cam/image_array', 'cam/depth_array'],
threaded=True)

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