Add support from Hokuyo lidar

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

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

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']