上传文件至 src/rc_lidar

src/rc_lidar/simple.py

@@ -0,0 +1,102 @@
+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 BasketballFrameDetector(Node):
+    def __init__(self):
+        super().__init__('basketball_frame_detector')
+        self.subscription = self.create_subscription(
+            PointCloud2,
+            '/livox/lidar_filtered',
+            self.pointcloud_callback,
+            10
+        )
+        self.publisher = self.create_publisher(
+            PointCloud2,
+            '/basketball_frame_cloud',
+            10
+        )
+        self.pointcloud_buffer = []
+
+    def pointcloud_callback(self, msg):
+        points = self.pointcloud2_to_xyz(msg)
+        self.pointcloud_buffer.append(points)
+        self.get_logger().info(f'已保存点云组数: {len(self.pointcloud_buffer)}')
+        if len(self.pointcloud_buffer) < 10:
+            return
+
+        # 合并10组点云
+        all_points = np.vstack(self.pointcloud_buffer)
+        xy_points = all_points[:, :2]
+        self.get_logger().info(f'合并后点数: {xy_points.shape[0]}')
+
+        # 清空缓存，准备下一批
+        self.pointcloud_buffer = []
+
+        # 聚类识别
+        if len(xy_points) < 10:
+            return
+        clustering = DBSCAN(eps=0.3, min_samples=10).fit(xy_points)
+        labels = clustering.labels_
+        unique_labels = set(labels)
+        for label in unique_labels:
+            if label == -1:
+                continue
+            cluster = all_points[labels == label]
+            if len(cluster) < 30:
+                continue
+            min_x, min_y = np.min(cluster[:, :2], axis=0)
+            max_x, max_y = np.max(cluster[:, :2], axis=0)
+            width = abs(max_x - min_x)
+            height = abs(max_y - min_y)
+            self.get_logger().info(
+                f'聚类: label={label}, width={width:.2f}, height={height:.2f}, 点数={len(cluster)}'
+            )
+            if 1.6 < width < 2.0 and 0.8 < height < 1.2:
+                self.get_logger().info(
+                    f'可能是篮球框: label={label}, width={width:.2f}, height={height:.2f}, 点数={len(cluster)}'
+                )
+                # 发布识别到的篮球框点云
+                cloud_msg = self.xyz_array_to_pointcloud2(cluster, msg.header)
+                self.publisher.publish(cloud_msg)
+
+    def pointcloud2_to_xyz(self, cloud_msg):
+        fmt = 'ffff'  # x, y, z, intensity
+        points = []
+        for i in range(cloud_msg.width):
+            offset = i * cloud_msg.point_step
+            x, y, z, intensity = struct.unpack_from(fmt, cloud_msg.data, offset)
+            points.append([x, y, z, intensity])
+        return np.array(points)
+
+    def xyz_array_to_pointcloud2(self, points, header):
+        # 构造 PointCloud2 消息
+        msg = PointCloud2()
+        msg.header = header
+        msg.height = 1
+        msg.width = len(points)
+        msg.is_dense = True
+        msg.is_bigendian = False
+        msg.point_step = 16
+        msg.row_step = msg.point_step * msg.width
+        msg.fields = [
+            PointField(name='x', offset=0, datatype=7, count=1),
+            PointField(name='y', offset=4, datatype=7, count=1),
+            PointField(name='z', offset=8, datatype=7, count=1),
+            PointField(name='intensity', offset=12, datatype=7, count=1),
+        ]
+        msg.data = b''.join([struct.pack('ffff', *p) for p in points])
+        return msg
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = BasketballFrameDetector()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/rc_lidar/simple_icp.py

@@ -0,0 +1,126 @@
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import PointCloud2
+from sensor_msgs_py import point_cloud2
+from geometry_msgs.msg import PoseStamped
+import open3d as o3d
+import numpy as np
+from transforms3d.quaternions import mat2quat
+
+class PointCloudLocalization(Node):
+    def __init__(self):
+        super().__init__('point_cloud_localizer')
+        
+        # 加载参考点云地图 (PCD文件)
+        self.reference_map = o3d.io.read_point_cloud("/home/robofish/RC2025/lankuang.pcd")  # 替换为你的PCD文件路径
+        if not self.reference_map.has_points():
+            self.get_logger().error("Failed to load reference map!")
+            rclpy.shutdown()
+        
+        # 预处理参考地图
+        self.reference_map = self.reference_map.voxel_down_sample(voxel_size=0.05)
+        self.reference_map.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0)[0]
+        
+        # 创建ICP对象
+        self.icp = o3d.pipelines.registration.registration_icp
+        self.threshold = 0.5  # 匹配距离阈值 (米)
+        self.trans_init = np.identity(4)  # 初始变换矩阵
+        
+        # 订阅激光雷达点云
+        self.subscription = self.create_subscription(
+            PointCloud2,
+            '/livox/lidar_filtered',
+            self.lidar_callback,
+            10)
+        
+        # 发布估计位置
+        self.pose_pub = self.create_publisher(PoseStamped, '/estimated_pose', 10)
+        
+        self.get_logger().info("Point Cloud Localization Node Initialized!")
+
+    def ros_pc2_to_o3d(self, ros_cloud):
+        """将ROS PointCloud2转换为Open3D点云"""
+        # 提取xyz坐标
+        points = point_cloud2.read_points(ros_cloud, field_names=("x", "y", "z"), skip_nans=True)
+        xyz = np.array([ [p[0], p[1], p[2]] for p in points ], dtype=np.float32)
+        
+        if xyz.shape[0] == 0:
+            return None
+            
+        # 创建Open3D点云
+        pcd = o3d.geometry.PointCloud()
+        pcd.points = o3d.utility.Vector3dVector(xyz)
+        return pcd
+
+    def preprocess_pointcloud(self, pcd):
+        """点云预处理"""
+        # 降采样
+        pcd = pcd.voxel_down_sample(voxel_size=0.03)
+        
+        # 移除离群点
+        pcd, _ = pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=1.0)
+        
+        # 移除地面 (可选)
+        # plane_model, inliers = pcd.segment_plane(distance_threshold=0.1, ransac_n=3, num_iterations=100)
+        # pcd = pcd.select_by_index(inliers, invert=True)
+        
+        return pcd
+
+    def lidar_callback(self, msg):
+        """处理新的激光雷达数据"""
+        # 转换为Open3D格式
+        current_pcd = self.ros_pc2_to_o3d(msg)
+        if current_pcd is None:
+            self.get_logger().warn("Received empty point cloud!")
+            return
+        
+        # 预处理当前点云
+        current_pcd = self.preprocess_pointcloud(current_pcd)
+        
+        # 执行ICP配准
+        reg_result = self.icp(
+            current_pcd, self.reference_map, self.threshold,
+            self.trans_init,
+            o3d.pipelines.registration.TransformationEstimationPointToPoint(),
+            o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=50)
+        )
+        
+        # 更新变换矩阵
+        self.trans_init = reg_result.transformation
+        
+        # 提取位置和方向
+        translation = reg_result.transformation[:3, 3]
+        rotation_matrix = reg_result.transformation[:3, :3]
+        
+        # 转换为四元数
+        quaternion = mat2quat(reg_result.transformation[:3, :3])  # 注意返回顺序为 [w, x, y, z]
+        
+        # 发布位姿
+        pose_msg = PoseStamped()
+        pose_msg.header.stamp = self.get_clock().now().to_msg()
+        pose_msg.header.frame_id = "livox_frame"
+        pose_msg.pose.position.x = translation[0]
+        pose_msg.pose.position.y = translation[1]
+        pose_msg.pose.position.z = translation[2]
+        pose_msg.pose.orientation.x = quaternion[1]  # x
+        pose_msg.pose.orientation.y = quaternion[2]  # y
+        pose_msg.pose.orientation.z = quaternion[3]  # z
+        pose_msg.pose.orientation.w = quaternion[0]  # w
+        
+        self.pose_pub.publish(pose_msg)
+        self.get_logger().info(f"Estimated Position: x={translation[0]:.2f}, y={translation[1]:.2f}, z={translation[2]:.2f}")
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = PointCloudLocalization()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/rc_lidar/xiamian.py

@@ -0,0 +1,75 @@
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import PointCloud2
+from nav_msgs.msg import OccupancyGrid
+import numpy as np
+import struct
+import time
+
+class PointCloudToGrid(Node):
+    def __init__(self):
+        super().__init__('pointcloud_to_grid')
+        self.subscription = self.create_subscription(
+            PointCloud2,
+            '/livox/lidar_filtered',
+            self.pointcloud_callback,
+            10)
+        self.publisher = self.create_publisher(OccupancyGrid, '/lidar_grid', 10)
+        self.grid_size = 2000
+        self.resolution = 0.02
+        self.origin_x = -20.0
+        self.origin_y = -20.0 
+        self.points_buffer = []
+        self.last_header = None
+        # 定时器每0.5秒触发一次
+        self.timer = self.create_timer(0.5, self.publish_grid)
+ 
+    def pointcloud_callback(self, msg):
+        points = self.pointcloud2_to_xyz_array(msg)
+        self.points_buffer.append(points)
+        self.last_header = msg.header  # 保存最新header用于地图消息
+
+    def publish_grid(self):
+        if not self.points_buffer:
+            return
+        # 合并0.5秒内所有点
+        all_points = np.concatenate(self.points_buffer, axis=0)
+        grid = np.zeros((self.grid_size, self.grid_size), dtype=np.int8)
+        for x, y, z in all_points:
+            if z < 2.0:
+                ix = int((x - self.origin_x) / self.resolution)
+                iy = int((y - self.origin_y) / self.resolution)
+                if 0 <= ix < self.grid_size and 0 <= iy < self.grid_size:
+                    grid[iy, ix] = 100
+        grid_msg = OccupancyGrid()
+        if self.last_header:
+            grid_msg.header = self.last_header
+        grid_msg.info.resolution = self.resolution
+        grid_msg.info.width = self.grid_size
+        grid_msg.info.height = self.grid_size
+        grid_msg.info.origin.position.x = self.origin_x
+        grid_msg.info.origin.position.y = self.origin_y
+        grid_msg.data = grid.flatten().tolist()
+        self.publisher.publish(grid_msg)
+        self.points_buffer.clear()  # 清空缓存
+
+    def pointcloud2_to_xyz_array(self, cloud_msg):
+        # 解析 PointCloud2 数据为 numpy 数组
+        fmt = 'fff'  # x, y, z
+        point_step = cloud_msg.point_step
+        data = cloud_msg.data
+        points = []
+        for i in range(0, len(data), point_step):
+            x, y, z = struct.unpack_from(fmt, data, i)
+            points.append([x, y, z])
+        return np.array(points)
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = PointCloudToGrid()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/rc_lidar/zhaoban.py

@@ -0,0 +1,51 @@
+import numpy as np
+from sklearn.cluster import DBSCAN
+from scipy.optimize import leastsq
+import matplotlib.pyplot as plt
+
+def fit_circle(x, y):
+    # 拟合圆的函数
+    def calc_R(xc, yc):
+        return np.sqrt((x - xc)**2 + (y - yc)**2)
+    def f(c):
+        Ri = calc_R(*c)
+        return Ri - Ri.mean()
+    center_estimate = np.mean(x), np.mean(y)
+    center, _ = leastsq(f, center_estimate)
+    radius = calc_R(*center).mean()
+    return center[0], center[1], radius
+
+def find_circle(points, eps=0.5, min_samples=10):
+    # 聚类
+    clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(points)
+    labels = clustering.labels_
+    # 只取最大簇
+    unique, counts = np.unique(labels[labels != -1], return_counts=True)
+    if len(unique) == 0:
+        raise ValueError("未找到有效聚类")
+    main_cluster = unique[np.argmax(counts)]
+    cluster_points = points[labels == main_cluster]
+    x, y = cluster_points[:, 0], cluster_points[:, 1]
+    # 拟合圆
+    xc, yc, r = fit_circle(x, y)
+    return xc, yc, r, cluster_points
+
+if __name__ == "__main__":
+    # 示例数据
+    np.random.seed(0)
+    angle = np.linspace(0, 2 * np.pi, 100)
+    x = 5 + 3 * np.cos(angle) + np.random.normal(0, 0.1, 100)
+    y = 2 + 3 * np.sin(angle) + np.random.normal(0, 0.1, 100)
+    points = np.vstack((x, y)).T
+
+    xc, yc, r, cluster_points = find_circle(points)
+    print(f"圆心: ({xc:.2f}, {yc:.2f}), 半径: {r:.2f}")
+
+    # 可视化
+    plt.scatter(points[:, 0], points[:, 1], label='所有点')
+    plt.scatter(cluster_points[:, 0], cluster_points[:, 1], label='聚类点')
+    circle = plt.Circle((xc, yc), r, color='r', fill=False, label='拟合圆')
+    plt.gca().add_patch(circle)
+    plt.legend()
+    plt.axis('equal')
+    plt.show()