diff --git a/donkeycar/parts/lidar.py b/donkeycar/parts/lidar.py index 4eaf8d977..2136f91a4 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): + 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): ''' @@ -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 @@ -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 @@ -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 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 32c4a7168..3154d39cf 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 = cfg.LIDAR_MAX_DIST) + V.add(lidar, inputs=[], outputs=['lidar/dist_scan'], threaded=False) 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 @@ -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": + 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']