Gpu lidar fix (#165)

* Added lidardisplay for ubuntu

* organize test files

* use cylindrical gpu for test

* Match GPU lidar conventions to CPU lidar conventions

* GPU shader no longer uses the cam-1 inverse deprojection path for points

* GPU lidar readback metadata alignment. This avoids publishing stale point data with the current tick’s pose.

* gpu_cylindrical now uses the configured vertical FOV lower/upper bounds instead of assuming symmetry around zero.

* GPU lidar: replace the projection-to-pixel step with explicit perspective math.

* Fix partial FOV normalization issue in GPU lidar

* GPU lidar guard threads beyond the actual point count

* GPU lidar order ok, fixed from renderer not from lidar code

* Gpu lidar working 100%

Co-authored-by: Lucas Scheinkerman <37854249+LucasJSch@users.noreply.github.com>

Author: jonyMarino

client/python/example_user_scripts/depth_lidar.py

@@ -4,11 +4,11 @@
 import time
 from pathlib import Path
 
-import cv2
 import numpy as np
 
 from projectairsim import Drone, ProjectAirSimClient, World
-from projectairsim.lidar_utils import LidarDisplay
+from projectairsim.image_utils import ImageDisplay
+from projectairsim.lidar_utils import LidarTopDownRenderer
 from projectairsim.utils import projectairsim_log
 
 
@@ -72,13 +72,12 @@ def receive(self, cloud_msg):
 async def main():
     client = ProjectAirSimClient()
     lidar_cloud_stats = PointCloudStats("lidar1.lidar")
-    lidar_cloud_display = LidarDisplay(
-        win_name="Lidar PointCloud",
-        color_mode=LidarDisplay.COLOR_INTENSITY,
+    image_display = ImageDisplay(num_subwin=1, subwin_width=640, subwin_height=640)
+    lidar_cloud_window = "Lidar PointCloud"
+    lidar_cloud_renderer = LidarTopDownRenderer(
         width=640,
-        height=360,
-        x=40,
-        y=420,
+        height=640,
+        range_m=60.0,
     )
     lidar_cloud_topic = None
     drone = None
@@ -100,14 +99,19 @@ async def main():
             f"Subscribing lidar pointcloud stream: {lidar_cloud_topic}"
         )
 
-        lidar_cloud_display.start()
+        image_display.add_image(lidar_cloud_window, subwin_idx=0)
+        image_display.start()
 
         client.subscribe(
             lidar_cloud_topic,
             safe_topic_callback(
                 "lidar_cloud",
                 lambda _, lidar_msg: handle_lidar_cloud(
-                    lidar_msg, lidar_cloud_stats, lidar_cloud_display
+                    lidar_msg,
+                    lidar_cloud_stats,
+                    lidar_cloud_renderer,
+                    image_display,
+                    lidar_cloud_window,
                 ),
             ),
         )
@@ -156,15 +160,23 @@ async def main():
             drone.disarm()
             drone.disable_api_control()
 
-        lidar_cloud_display.stop()
-        cv2.destroyAllWindows()
+        image_display.stop()
         client.disconnect()
 
 
-def handle_lidar_cloud(lidar_msg, lidar_cloud_stats, lidar_cloud_display):
+def handle_lidar_cloud(
+    lidar_msg,
+    lidar_cloud_stats,
+    lidar_cloud_renderer,
+    image_display,
+    lidar_cloud_window,
+):
     lidar_cloud_stats.receive(lidar_msg)
-    lidar_cloud_display.receive(lidar_msg)
+    image_display.receive(
+        lidar_cloud_renderer.render_image_msg(lidar_msg),
+        lidar_cloud_window,
+    )
 
 
 if __name__ == "__main__":
-    asyncio.run(main())
+    asyncio.run(main())

client/python/example_user_scripts/lidar_basic.py

@@ -7,28 +7,60 @@
 """
 
 import asyncio
+import math
+import time
+from pathlib import Path
 
 from projectairsim import ProjectAirSimClient, Drone, World
+from projectairsim.lidar_utils import LidarTopDownRenderer
 from projectairsim.utils import projectairsim_log
 from projectairsim.image_utils import ImageDisplay
-from projectairsim.lidar_utils import LidarDisplay
+
+
+class LidarStats:
+    def __init__(self):
+        self.message_count = 0
+        self.last_report_wall = 0.0
+
+    def receive(self, lidar_data):
+        if lidar_data is None:
+            return
+
+        point_count = int(len(lidar_data.get("point_cloud", [])) / 3)
+        if point_count == 0:
+            return
+
+        self.message_count += 1
+        now = time.time()
+        if self.message_count == 1 or now - self.last_report_wall >= 1.0:
+            projectairsim_log().info(
+                f"lidar1: msg={self.message_count} points={point_count}"
+            )
+            self.last_report_wall = now
+
+
+def receive_lidar(lidar_data, lidar_stats, lidar_renderer, image_display, image_name):
+    lidar_stats.receive(lidar_data)
+    image_display.receive(lidar_renderer.render_image_msg(lidar_data), image_name)
 
 
 # Async main function to wrap async drone commands
 async def main():
     # Create a Project AirSim client
     client = ProjectAirSimClient()
+    lidar_stats = LidarStats()
 
     # Initialize an ImageDisplay object to display camera sub-windows
     image_display = ImageDisplay()
 
     # ----------------------------------------------------------------------------------
 
-    # Initialize a LidarDisplay object for a point cloud visualization sub-window
-    lidar_subwin = image_display.get_subwin_info(2)
-    lidar_display = LidarDisplay(
-        x=lidar_subwin["x"], y=lidar_subwin["y"] + 30
-    )  # add 30 y-pix for window title bar
+    # Initialize a top-down LIDAR image renderer.
+    lidar_renderer = LidarTopDownRenderer(
+        width=640,
+        height=640,
+        range_m=60.0,
+    )
 
     # ----------------------------------------------------------------------------------
 
@@ -37,7 +69,13 @@ async def main():
         client.connect()
 
         # Create a World object to interact with the sim world and load a scene
-        world = World(client, "scene_lidar_drone.jsonc", delay_after_load_sec=2)
+        sim_config_path = str(Path(__file__).resolve().parent / "sim_config")
+        world = World(
+            client,
+            "scene_lidar_drone.jsonc",
+            delay_after_load_sec=2,
+            sim_config_path=sim_config_path,
+        )
 
         # Create a Drone object to interact with a drone in the loaded sim world
         drone = Drone(client, world, "Drone1")
@@ -65,22 +103,24 @@ async def main():
             lambda _, depth: image_display.receive(depth, depth_name),
         )
 
+        lidar_name = "LIDAR"
+        image_display.add_image(lidar_name, resize_x=640, resize_y=640)
+
         image_display.start()
 
         # ------------------------------------------------------------------------------
 
         client.subscribe(
             drone.sensors["lidar1"]["lidar"],
-            lambda _, lidar: lidar_display.receive(lidar),
+            lambda _, lidar: receive_lidar(
+                lidar, lidar_stats, lidar_renderer, image_display, lidar_name
+            ),
         )
 
         # client.subscribe(
         #     drone.sensors["lidar1"]["lidar"],
         #     lambda _, lidar: print(lidar),
         # )
-
-        lidar_display.start()
-
         # ------------------------------------------------------------------------------
 
         # Set the drone to be ready to fly
@@ -100,6 +140,13 @@ async def main():
         )
         await move_task
 
+        projectairsim_log().info("Yaw 720 degrees")
+        yaw_task = await drone.rotate_by_yaw_rate_async(
+            yaw_rate=math.radians(12.0),
+            duration=20.0,
+        )
+        await yaw_task
+
         projectairsim_log().info("Move north-east")
         move_task = await drone.move_by_velocity_async(
             v_north=4.0, v_east=4.0, v_down=0.0, duration=8.0
@@ -133,10 +180,6 @@ async def main():
 
         # ------------------------------------------------------------------------------
 
-        lidar_display.stop()
-
-        # ------------------------------------------------------------------------------
-
 
 if __name__ == "__main__":
     asyncio.run(main())  # Runner for async main function

client/python/example_user_scripts/sim_config/robot_quadrotor_fastphysics_lidar.jsonc

@@ -424,25 +424,27 @@
     {
       "id": "lidar1",
       "type": "lidar",
+      "lidar-type": "gpu_cylindrical",
       "enabled": true,
       "parent-link": "Frame",
-      "number-of-channels": 16,
+      "number-of-channels": 32,
       "range": 100,
-      "points-per-second": 100000,
+      "points-per-second": 180000,
       "horizontal-rotation-frequency": 10,
-      "horizontal-fov-start-deg": 0.0,
+      "horizontal-fov-start-deg": 0,
       "horizontal-fov-end-deg": 360.0,
-      "vertical-fov-upper-deg": 0.0,
-      "vertical-fov-lower-deg": -90.0,
+      "vertical-fov-upper-deg": 15.0,
+      "vertical-fov-lower-deg": -15.0,
       "disable-self-hits": true,
       "draw-debug-points": false,
-      "origin": {
-        "xyz": "0 0 0.2",
-        "rpy-deg": "0 0 0"
-      },
+      "report-frequency": 0,
       "report-point-cloud": true,
       "report-azimuth-elevation-range": false,
-      "report-no-return-points": false
+      "report-no-return-points": false,
+      "origin": {
+        "xyz": "0 0 1.0",
+        "rpy-deg": "0 0 0"
+      }
     },
     {
       "id": "GPS",
@@ -451,4 +453,4 @@
       "parent-link": "Frame"
     }
   ]
-}
+}

client/python/projectairsim/src/projectairsim/lidar_utils.py

@@ -2,6 +2,7 @@
 import multiprocessing as mp
 import time
 
+import cv2
 import numpy as np
 import open3d as o3d
 from matplotlib import cm
@@ -10,6 +11,74 @@
 # from projectairsim.utils import projectairsim_log
 
 
+class LidarTopDownRenderer:
+    """Render lidar point clouds as top-down OpenCV-compatible image messages.
+
+    The output uses the sensor frame with forward plotted upward and right plotted
+    to the right. Point color is based on the down-axis value.
+    """
+
+    def __init__(self, width=640, height=640, range_m=60.0):
+        self.width = width
+        self.height = height
+        self.range_m = range_m
+
+    def make_blank_image(self):
+        image = np.zeros((self.height, self.width, 3), dtype=np.uint8)
+        center = (self.width // 2, self.height // 2)
+        cv2.circle(image, center, 3, (255, 255, 255), -1)
+        return image
+
+    def render_lidar(self, lidar_data):
+        point_cloud = lidar_data.get("point_cloud")
+        if point_cloud is None or len(point_cloud) == 0:
+            return self.make_blank_image()
+
+        points = np.asarray(point_cloud, dtype=np.float32).reshape((-1, 3))
+        image = self.make_blank_image()
+
+        scale = min(self.width, self.height) / (2.0 * self.range_m)
+        px = np.round(self.width / 2.0 + points[:, 1] * scale).astype(np.int32)
+        py = np.round(self.height / 2.0 - points[:, 0] * scale).astype(np.int32)
+
+        in_bounds = (px >= 0) & (px < self.width) & (py >= 0) & (py < self.height)
+        if not np.any(in_bounds):
+            return image
+
+        z_down = points[in_bounds, 2]
+        z_norm = np.clip((z_down - z_down.min()) / (np.ptp(z_down) + 1e-6), 0.0, 1.0)
+        colors = cv2.applyColorMap(
+            (255.0 * z_norm).astype(np.uint8), cv2.COLORMAP_TURBO
+        )
+        image[py[in_bounds], px[in_bounds]] = colors[:, 0, :]
+
+        cv2.line(
+            image,
+            (self.width // 2, 0),
+            (self.width // 2, self.height - 1),
+            (60, 60, 60),
+            1,
+        )
+        cv2.line(
+            image,
+            (0, self.height // 2),
+            (self.width - 1, self.height // 2),
+            (60, 60, 60),
+            1,
+        )
+        cv2.circle(image, (self.width // 2, self.height // 2), 4, (255, 255, 255), -1)
+        return image
+
+    def render_image_msg(self, lidar_data):
+        image = self.render_lidar(lidar_data)
+        return {
+            "encoding": "BGR",
+            "height": self.height,
+            "width": self.width,
+            "data": image.tobytes(),
+        }
+
+
 class LidarDisplay:
     """Class to display LIDAR point cloud data in a window. This runs its visualization
     display update loop in a separate process independent of the client's async.io main