上传文件至 src/rc_lidar

src/rc_lidar/caijian.py

@@ -0,0 +1,63 @@
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import PointCloud2, PointField
+import numpy as np
+import struct
+from sklearn.cluster import DBSCAN
+
+class LidarFilterNode(Node):
+    def __init__(self):
+        super().__init__('caijian_node')
+        self.publisher_ = self.create_publisher(PointCloud2, '/livox/lidar_filtered', 10)
+        self.subscription = self.create_subscription(
+            PointCloud2,
+            '/livox/lidar/pointcloud',
+            self.filter_callback,
+            10)
+        self.get_logger().info('caijian_node started, numpy filtering z in [1.5,3]m, distance<=12m, remove isolated points')
+
+    def filter_callback(self, msg):
+        num_points = msg.width * msg.height
+        data = np.frombuffer(msg.data, dtype=np.uint8)
+        points = np.zeros((num_points, 4), dtype=np.float32)  # x, y, z, intensity
+
+        for i in range(num_points):
+            offset = i * msg.point_step
+            x = struct.unpack_from('f', data, offset)[0]
+            y = struct.unpack_from('f', data, offset + 4)[0]
+            z = struct.unpack_from('f', data, offset + 8)[0]
+            intensity = struct.unpack_from('f', data, offset + 12)[0]
+            points[i] = [x, y, z, intensity]
+
+        z_mask = (points[:,2] >= 1.5) & (points[:,2] <= 3.0)
+        dist_mask = np.linalg.norm(points[:,:3], axis=1) <= 16.0
+        mask = z_mask & dist_mask
+        filtered_points = points[mask]
+        
+        # 使用DBSCAN去除孤立点
+        if filtered_points.shape[0] > 0:
+            clustering = DBSCAN(eps=0.3, min_samples=5).fit(filtered_points[:,:3])
+            core_mask = clustering.labels_ != -1
+            filtered_points = filtered_points[core_mask]
+
+        fields = [
+            PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
+            PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
+            PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
+            PointField(name='intensity', offset=12, datatype=PointField.FLOAT32, count=1),
+        ]
+        filtered_points_list = filtered_points.tolist()
+        import sensor_msgs_py.point_cloud2 as pc2
+        filtered_msg = pc2.create_cloud(msg.header, fields, filtered_points_list)
+        self.publisher_.publish(filtered_msg)
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = LidarFilterNode()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()