added everything from pc

scripts/calibrate_lidar.py

@@ -13,6 +13,7 @@ import threading
 import time
 import open3d as o3d
 from scipy.spatial.transform import Rotation as R
+from datetime import datetime
 
 class PointCloudSaver(Node):
     def __init__(self, node_name: str, pointcloud_topic: str, buffer, timeout_ms: int):
@@ -111,6 +112,11 @@ class LidarTransformPublisher(Node):
 
         self.br.sendTransform(t)
         self.get_logger().info("Published static transform.")
+        if input("Is it acurate? [y/N] ").lower() == 'y':
+            file_name = f"calibration/{datetime.now().strftime("%Y_%m_%d-%H_%M_%S")}.npy"
+            np.save(file_name, self.T)
+            self.get_logger().info(f"Saved {file_name}")
+        
 
     def pcd_buffer_to_o3d(self, buffer: BytesIO):
         buffer.seek(0)
@@ -195,7 +201,7 @@ class LidarTransformPublisher(Node):
         return transformation
 
 
-def monitor_nodes(nodes, publisher):
+def monitor_nodes(nodes, executor, publisher):
     while rclpy.ok():
         if all(node.finished for node in nodes):
             print("All pointclouds captured")
@@ -210,12 +216,12 @@ def main():
 
     executor = MultiThreadedExecutor()
     nodes = [
-        PointCloudSaver('velodyne_pcd_saver', '/velodyne_points', buffer_velodyne, 350),
-        PointCloudSaver('livox_pcd_saver', '/livox/lidar', buffer_livox, 1000),
+        PointCloudSaver('velodyne_pcd_saver', '/lidar/vlp16/points', buffer_velodyne, 3_500),
+        PointCloudSaver('livox_pcd_saver', '/lidar/mid40/points', buffer_livox, 5_000),
     ]
     publisher = LidarTransformPublisher(buffer_velodyne, 'velodyne', buffer_livox, 'frame_default')
 
-    monitor_thread = threading.Thread(target=monitor_nodes, args=(nodes,publisher), daemon=True)
+    monitor_thread = threading.Thread(target=monitor_nodes, args=(nodes,executor,publisher), daemon=True)
     monitor_thread.start()
 
     for node in nodes: