dual lidar calibration

scripts/save_pointcloud.py

@@ -1,11 +1,14 @@
 import rclpy
 from rclpy.node import Node
+from rclpy.executors import MultiThreadedExecutor
 from sensor_msgs.msg import PointCloud2
 import sensor_msgs_py.point_cloud2 as pc2
 import numpy as np
 import matplotlib.pyplot as plt
 import struct
 from io import BytesIO
+import threading
+import time
 
 class PointCloudSaver(Node):
     def __init__(self, node_name: str, pointcloud_topic: str, buffer, timeout_ms: int):
@@ -17,6 +20,7 @@ class PointCloudSaver(Node):
             10
         )
         self.buffer = buffer
+        self.finished = False
         self.points = []
         self.end_time = self.get_clock().now().nanoseconds + (timeout_ms * 1_000_000)
         self.cmap = plt.get_cmap('jet') 
@@ -29,7 +33,8 @@ class PointCloudSaver(Node):
         if now > self.end_time:
             if not self.points:
                 self.get_logger().warn("No points received!")
-                rclpy.shutdown()
+                self.destroy_node()
+                self.finished = True
                 return
 
             np_points = np.array(self.points, dtype=np.float32)
@@ -45,8 +50,9 @@ class PointCloudSaver(Node):
 
             filename = "pointcloud.pcd"
             self.write_pcd_with_intensity_rgb(filename, np_points, rgb_int)
-            self.get_logger().info(f"Saved {filename} with intensity (grayscale) and colored RGB")
-            rclpy.shutdown()
+            self.get_logger().info(f"Saved {filename}")
+            self.destroy_node()
+            self.finished = True
 
     def write_pcd_with_intensity_rgb(self, filename, points, rgb_int):
         header = f"""# .PCD v0.7 - Point Cloud Data file format
@@ -64,13 +70,43 @@ DATA binary
         self.buffer.write(header.encode('ascii'))
         for i in range(points.shape[0]):
             # x, y, z, intensity as float32, rgb as uint32
-            f.write(struct.pack('ffffI', points[i,0], points[i,1], points[i,2], points[i,3], rgb_int[i]))
+            self.buffer.write(struct.pack('ffffI', points[i,0], points[i,1], points[i,2], points[i,3], rgb_int[i]))
+
+def monitor_nodes(nodes):
+    """Separate thread that monitors node status and shuts down ROS when done."""
+    while rclpy.ok():
+        if all(node.finished for node in nodes):
+            print("All nodes finished. Shutting down ROS.")
+            rclpy.shutdown()
+            break
+        time.sleep(0.1)  # check periodically
 
 def main():
     rclpy.init()
-    with open('/root/velodyne.pcd', "w+") as f:
-        node = PointCloudSaver('velodyne_pcd_saver', '/velodyne_points', f, 5000)
-    rclpy.spin(node)
+    file_velodyne = open('/root/velodyne.pcd', "wb+")
+    file_livox = open('/root/livox.pcd', "wb+")
+
+    executor = MultiThreadedExecutor()
+
+    nodes = [
+        PointCloudSaver('velodyne_pcd_saver', '/velodyne_points', file_velodyne, 5000),
+        PointCloudSaver('livox_pcd_saver', '/livox/lidar', file_livox, 5000),
+    ]
+
+    monitor_thread = threading.Thread(target=monitor_nodes, args=(nodes,), daemon=True)
+    monitor_thread.start()
+
+    for node in nodes:
+        executor.add_node(node)
+    try:
+        executor.spin()
+    finally:
+        monitor_thread.join()
+        print("Executor and monitor thread exited cleanly.")
+
+        file_velodyne.close()
+        file_livox.close()
+
 
 if __name__ == "__main__":
     main()