lidar viz as as camera, fixes

autorope · Jun 11, 2024 · e526ce7 · e526ce7
1 parent 05089ae
commit e526ce7
Show file tree

Hide file tree

Showing 3 changed files with 41 additions and 10 deletions.
diff --git 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
@@ -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
@@ -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']