From 05089aeabe3222de662f9d4da498e48288f54c6f Mon Sep 17 00:00:00 2001 From: Manav Gagvani Date: Sat, 1 Jun 2024 17:13:10 -0400 Subject: [PATCH 1/3] [UNTESTED] Add support for Hokuyo lidar in donkeycar/parts/lidar.py and donkeycar/templates/complete.py --- donkeycar/parts/lidar.py | 43 ++++++++++++++++++++++++++++++--- donkeycar/templates/complete.py | 10 +++++++- 2 files changed, 48 insertions(+), 5 deletions(-) diff --git a/donkeycar/parts/lidar.py b/donkeycar/parts/lidar.py index 4eaf8d977..73533f8c2 100644 --- a/donkeycar/parts/lidar.py +++ b/donkeycar/parts/lidar.py @@ -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): + # 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) + + def update(self): + self.poll() + time.sleep(0) # copied from RPLidar2 + + def run_threaded(self): + return self.scan + + def run(self): + self.poll() + return self.scan + + def shutdown(self): + self.laser.close() + class LidarPlot(object): ''' @@ -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 @@ -708,7 +743,7 @@ def run(self, measurements): [(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 @@ -893,8 +928,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 diff --git a/donkeycar/templates/complete.py b/donkeycar/templates/complete.py index 32c4a7168..6245e699d 100644 --- a/donkeycar/templates/complete.py +++ b/donkeycar/templates/complete.py @@ -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 = 10_000) + V.add(lidar, inputs=[], outputs=['lidar/dist_scan'], threaded=True) if cfg.HAVE_TFMINI: from donkeycar.parts.tfmini import TFMini @@ -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 From e526ce7e38610fe14e0405a45a349ef62f27beb7 Mon Sep 17 00:00:00 2001 From: Manav Gagvani Date: Sun, 2 Jun 2024 11:51:16 -0400 Subject: [PATCH 2/3] lidar viz as as camera, fixes --- donkeycar/parts/lidar.py | 17 +++++++++++------ donkeycar/templates/cfg_complete.py | 6 ++++-- donkeycar/templates/complete.py | 28 ++++++++++++++++++++++++++-- 3 files changed, 41 insertions(+), 10 deletions(-) diff --git a/donkeycar/parts/lidar.py b/donkeycar/parts/lidar.py index 73533f8c2..d86a35374 100644 --- a/donkeycar/parts/lidar.py +++ b/donkeycar/parts/lidar.py @@ -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 @@ -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: + print("bruh") + return np.asarray(self.frame) + # data points plot_polar_points( draw, bounds, self.mark_fn, self.point_color, self.mark_px, diff --git a/donkeycar/templates/cfg_complete.py b/donkeycar/templates/cfg_complete.py index d468f6b3c..8965bfeda 100644 --- a/donkeycar/templates/cfg_complete.py +++ b/donkeycar/templates/cfg_complete.py @@ -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 @@ -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 +LIDAR_ANGLE_OFFSET = 135 # The zero point of the LiDAR is rotated this much in degrees on LidarPlot # TFMINI HAVE_TFMINI = False diff --git a/donkeycar/templates/complete.py b/donkeycar/templates/complete.py index 6245e699d..7fc859496 100644 --- a/donkeycar/templates/complete.py +++ b/donkeycar/templates/complete.py @@ -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 @@ -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": + 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": + 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'] From 435d3fb751369b5d053677f03d6efa1d66558763 Mon Sep 17 00:00:00 2001 From: Manav Gagvani Date: Mon, 1 Jul 2024 15:00:04 +0000 Subject: [PATCH 3/3] fixes addressing @Ezward review --- donkeycar/parts/lidar.py | 10 ++++++---- donkeycar/templates/complete.py | 9 --------- 2 files changed, 6 insertions(+), 13 deletions(-) diff --git a/donkeycar/parts/lidar.py b/donkeycar/parts/lidar.py index d86a35374..2136f91a4 100644 --- a/donkeycar/parts/lidar.py +++ b/donkeycar/parts/lidar.py @@ -737,10 +737,12 @@ 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: - print("bruh") - return np.asarray(self.frame) + # 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( diff --git a/donkeycar/templates/complete.py b/donkeycar/templates/complete.py index 7fc859496..3154d39cf 100644 --- a/donkeycar/templates/complete.py +++ b/donkeycar/templates/complete.py @@ -882,15 +882,6 @@ def add_camera(V, cfg, camera_type): 'imu/gyr_x', 'imu/gyr_y', 'imu/gyr_z'], threaded=False) - elif cfg.CAMERA_TYPE == "OAKD": - 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": from donkeycar.parts.lidar import LidarPlot2