diff --git a/scripts/publish_lidar_offset.py b/scripts/publish_lidar_offset.py new file mode 100644 index 0000000..fce06d1 --- /dev/null +++ b/scripts/publish_lidar_offset.py @@ -0,0 +1,87 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from tf2_ros import StaticTransformBroadcaster +from geometry_msgs.msg import TransformStamped +import numpy as np +import open3d as o3d +from scipy.spatial.transform import Rotation as R + +class StaticTransformPublisher(Node): + def __init__(self): + super().__init__('static_transform_from_pointclouds') + + # Static TF broadcaster + self.br = StaticTransformBroadcaster(self) + + # Pointcloud files + self.velodyne_file = "/root/velodyne.pcd" + self.livox_file = "/root/livox.pcd" + + # Frames + self.livox_frame = "frame_default" + self.velodyne_frame = "velodyne" + + # Compute transform once + self.T = self.compute_transform() + self.get_logger().info(f"Computed initial transform:\n{self.T}") + + # Prepare translation and rotation + T_copy = np.array(self.T, copy=True) + trans = T_copy[:3, 3] + rot_quat = R.from_matrix(T_copy[:3, :3]).as_quat() # [x, y, z, w] + + # Create static TransformStamped + t = TransformStamped() + t.header.stamp.sec = 0 + t.header.stamp.nanosec = 0 + t.header.frame_id = self.velodyne_frame + t.child_frame_id = self.livox_frame + t.transform.translation.x = trans[0] + t.transform.translation.y = trans[1] + t.transform.translation.z = trans[2] + t.transform.rotation.x = rot_quat[0] + t.transform.rotation.y = rot_quat[1] + t.transform.rotation.z = rot_quat[2] + t.transform.rotation.w = rot_quat[3] + + # Publish once + self.br.sendTransform(t) + self.get_logger().info("Published static transform.") + + def compute_transform(self): + # Load point clouds + pcd_vel = o3d.io.read_point_cloud(self.velodyne_file) + pcd_liv = o3d.io.read_point_cloud(self.livox_file) + + # Downsample + voxel_size = 0.05 + pcd_vel_ds = pcd_vel.voxel_down_sample(voxel_size) + pcd_liv_ds = pcd_liv.voxel_down_sample(voxel_size) + + # Estimate normals + pcd_vel_ds.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamKNN(knn=20)) + pcd_liv_ds.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamKNN(knn=20)) + + # ICP registration + threshold = 0.5 + reg_result = o3d.pipelines.registration.registration_icp( + pcd_liv_ds, pcd_vel_ds, threshold, + np.eye(4), + o3d.pipelines.registration.TransformationEstimationPointToPoint() + ) + return reg_result.transformation + +def main(args=None): + rclpy.init(args=args) + node = StaticTransformPublisher() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/scripts/save_pointcloud.py b/scripts/save_pointcloud.py index c91eb8b..4526c17 100644 --- a/scripts/save_pointcloud.py +++ b/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()