r1-7-12

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()

src/rc_lidar/circlr.py

@@ -0,0 +1,90 @@
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import PointCloud2
+from geometry_msgs.msg import PointStamped
+from sensor_msgs_py import point_cloud2 as pc2
+import numpy as np
+from sklearn.cluster import DBSCAN
+import time
+
+def statistical_outlier_removal(points, k=20, std_ratio=2.0):
+    from sklearn.neighbors import NearestNeighbors
+    nbrs = NearestNeighbors(n_neighbors=k+1).fit(points)
+    distances, _ = nbrs.kneighbors(points)
+    mean_dist = np.mean(distances[:, 1:], axis=1)
+    threshold = np.mean(mean_dist) + std_ratio * np.std(mean_dist)
+    mask = mean_dist < threshold
+    return points[mask]
+
+class HoopFinder(Node):
+    def __init__(self):
+        super().__init__('find_hoop')
+        self.sub = self.create_subscription(
+            PointCloud2,
+            '/livox/lidar_filtered',
+            self.callback,
+            10)
+        self.pub = self.create_publisher(PointStamped, '/hoop_position', 10)
+        self.buffer = []
+        self.start_time = None
+        self.hoop_history = []
+
+    def callback(self, msg):
+        # 采集0.4秒内的点云
+        if self.start_time is None:
+            self.start_time = time.time()
+        for p in pc2.read_points(msg, field_names=("x", "y", "z", "intensity"), skip_nans=True):
+            self.buffer.append([p[0], p[1], p[2], p[3]])
+        if time.time() - self.start_time < 0.4:
+            return
+
+        points = np.array(self.buffer)
+        self.buffer = []
+        self.start_time = None
+
+        # 高度滤波
+        filtered = points[(points[:,2] > 1.0) & (points[:,2] < 3.0)]
+        if len(filtered) == 0:
+            return
+
+        # 统计离群点滤波
+        filtered = statistical_outlier_removal(filtered[:,:3], k=20, std_ratio=2.0)
+
+        # DBSCAN聚类
+        clustering = DBSCAN(eps=0.3, min_samples=10).fit(filtered)
+        labels = clustering.labels_
+        unique_labels = set(labels)
+        hoop_pos = None
+        max_cluster_size = 0
+
+        for label in unique_labels:
+            if label == -1:
+                continue
+            cluster = filtered[labels == label]
+            if len(cluster) > max_cluster_size:
+                max_cluster_size = len(cluster)
+                hoop_pos = np.mean(cluster, axis=0)
+
+        # 均值滤波输出
+        if hoop_pos is not None:
+            self.hoop_history.append(hoop_pos)
+            if len(self.hoop_history) > 5:
+                self.hoop_history.pop(0)
+            smooth_pos = np.mean(self.hoop_history, axis=0)
+            pt = PointStamped()
+            pt.header = msg.header
+            pt.point.x = float(smooth_pos[0])
+            pt.point.y = float(smooth_pos[1])
+            pt.point.z = float(smooth_pos[2])
+            self.pub.publish(pt)
+            self.get_logger().info(f"Hoop position (smoothed): {smooth_pos}")
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = HoopFinder()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/rc_lidar/find.py

@@ -0,0 +1,59 @@
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import PointCloud2
+from geometry_msgs.msg import PointStamped
+from sensor_msgs_py import point_cloud2 as pc2
+import numpy as np
+from sklearn.cluster import DBSCAN
+
+class HoopFinder(Node):
+    def __init__(self):
+        super().__init__('find_hoop')
+        self.sub = self.create_subscription(
+            PointCloud2,
+            '/livox/lidar',
+            self.callback,
+            10)
+        self.pub = self.create_publisher(PointStamped, '/hoop_position', 10)
+
+    def callback(self, msg):
+        points = []
+        for p in pc2.read_points(msg, field_names=("x", "y", "z", "intensity"), skip_nans=True):
+            points.append([p[0], p[1], p[2], p[3]])
+        points = np.array(points)
+        filtered = points[(points[:,2] > 1.0) & (points[:,2] < 3.0)]
+        if len(filtered) == 0:
+            return
+
+        clustering = DBSCAN(eps=0.3, min_samples=10).fit(filtered[:,:3])
+        labels = clustering.labels_
+        unique_labels = set(labels)
+        hoop_pos = None
+        max_cluster_size = 0
+
+        for label in unique_labels:
+            if label == -1:
+                continue
+            cluster = filtered[labels == label]
+            if len(cluster) > max_cluster_size:
+                max_cluster_size = len(cluster)
+                hoop_pos = np.mean(cluster[:,:3], axis=0)
+
+        if hoop_pos is not None:
+            pt = PointStamped()
+            pt.header = msg.header
+            pt.point.x = float(hoop_pos[0])
+            pt.point.y = float(hoop_pos[1])
+            pt.point.z = float(hoop_pos[2])
+            self.pub.publish(pt)
+            self.get_logger().info(f"Hoop position: {hoop_pos}")
+            
+def main(args=None):
+    rclpy.init(args=args)
+    node = HoopFinder()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/rc_lidar/fliter.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) <= 12.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()

src/rc_lidar/juxing.py

@@ -0,0 +1,340 @@
+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
+import cv2
+from visualization_msgs.msg import Marker
+from sklearn.linear_model import RANSACRegressor
+
+
+def ransac_line_3d(points, threshold=0.05, min_inliers=20):
+    best_inliers = []
+    best_line = None
+    N = len(points)
+    if N < min_inliers:
+        return None, []
+    for _ in range(100):
+        idx = np.random.choice(N, 2, replace=False)
+        p1, p2 = points[idx]
+        v = p2 - p1
+        v = v / np.linalg.norm(v)
+        dists = np.linalg.norm(np.cross(points - p1, v), axis=1)
+        inliers = np.where(dists < threshold)[0]
+        if len(inliers) > len(best_inliers):
+            best_inliers = inliers
+            best_line = (p1, p2)
+        if len(best_inliers) > N * 0.5:
+            break
+    return best_line, best_inliers
+
+def fit_rectangle_pca(cluster):
+    # 用PCA找主方向和边界点
+    pts = cluster[:, :3]
+    mean = np.mean(pts, axis=0)
+    cov = np.cov(pts.T)
+    eigvals, eigvecs = np.linalg.eigh(cov)
+    order = np.argsort(eigvals)[::-1]
+    main_dir = eigvecs[:, order[0]]
+    second_dir = eigvecs[:, order[1]]
+    # 投影到主方向和次方向
+    proj_main = np.dot(pts - mean, main_dir)
+    proj_second = np.dot(pts - mean, second_dir)
+    # 找四个角点
+    corners = []
+    for xm in [np.min(proj_main), np.max(proj_main)]:
+        for xs in [np.min(proj_second), np.max(proj_second)]:
+            idx = np.argmin((proj_main - xm)**2 + (proj_second - xs)**2)
+            corners.append(pts[idx])
+    return corners
+
+def rectangle_score(pts):
+    # 评估4个点是否接近矩形
+    d = [np.linalg.norm(pts[i] - pts[(i+1)%4]) for i in range(4)]
+    diag1 = np.linalg.norm(pts[0] - pts[2])
+    diag2 = np.linalg.norm(pts[1] - pts[3])
+    w = max(d)
+    h = min(d)
+    ratio = w / h if h > 0 else 0
+    ideal_ratio = 1.8 / 1.05
+    score = abs(ratio - ideal_ratio) + abs(diag1 - diag2) / max(diag1, diag2)
+    return score
+    
+def classify_lines(lines):
+    # lines: 每条线是 [x, y, z, intensity]
+    # 假设 lines 是8个端点，两两为一条线
+    vertical_lines = []
+    horizontal_lines = []
+    for i in range(0, len(lines), 2):
+        p1 = np.array(lines[i][:3])
+        p2 = np.array(lines[i+1][:3])
+        vec = p2 - p1
+        length = np.linalg.norm(vec)
+        # 计算与地面的夹角（假设地面为z轴为0，垂直为z方向）
+        dz = abs(vec[2])
+        dxy = np.linalg.norm(vec[:2])
+        # 垂直线：z方向分量大，长度约1.05m
+        if dz > dxy and 0.95 < length < 1.15:
+            vertical_lines.append((i, i+1, length))
+        # 水平线：z方向分量小，长度约1.8m
+        elif dz < dxy and 1.7 < length < 1.9:
+            horizontal_lines.append((i, i+1, length))
+    return vertical_lines, horizontal_lines
+
+def find_best_rectangle_from_lines(lines):
+    vertical_lines, horizontal_lines = classify_lines(lines)
+    # 只选出2条垂直线和2条水平线
+    if len(vertical_lines) < 2 or len(horizontal_lines) < 2:
+        return None
+    # 取长度最接近目标的两条
+    vertical_lines = sorted(vertical_lines, key=lambda x: abs(x[2]-1.05))[:2]
+    horizontal_lines = sorted(horizontal_lines, key=lambda x: abs(x[2]-1.8))[:2]
+    # 组合4个端点
+    indices = [vertical_lines[0][0], vertical_lines[0][1],
+                vertical_lines[1][0], vertical_lines[1][1],
+                horizontal_lines[0][0], horizontal_lines[0][1],
+                horizontal_lines[1][0], horizontal_lines[1][1]]
+    # 去重，只保留4个顶点
+    unique_indices = list(set(indices))
+    if len(unique_indices) < 4:
+        return None
+    pts = [lines[idx] for idx in unique_indices[:4]]
+    # 按矩形评分排序
+    from itertools import permutations
+    best_score = float('inf')
+    best_rect = None
+    for order in permutations(range(4)):
+        ordered = [pts[i] for i in order]
+        score = rectangle_score(np.array([p[:3] for p in ordered]))
+        if score < best_score:
+            best_score = score
+            best_rect = ordered
+    return best_rect
+    
+
+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.marker_pub = self.create_publisher(
+            Marker,
+            '/basketball_frame_lines',
+            10
+        )
+        self.pointcloud_buffer = []
+        self.max_buffer_size = 10  # 减少缓冲帧数，加快响应
+        self.center_buffer = []
+        self.center_buffer_size = 5
+
+    def pointcloud_callback(self, msg):
+        points = self.pointcloud2_to_xyz(msg)
+        if points.shape[0] == 0:
+            return  # 跳过空点云
+        self.pointcloud_buffer.append(points)
+        # 只保留非空点云
+        self.pointcloud_buffer = [arr for arr in self.pointcloud_buffer if arr.shape[0] > 0]
+        if len(self.pointcloud_buffer) > self.max_buffer_size:
+            self.pointcloud_buffer.pop(0)
+        all_points = np.vstack(self.pointcloud_buffer)
+        xy_points = all_points[:, :2]
+
+        if len(xy_points) < 10:
+            self.get_logger().info('点数太少，跳过')
+            return
+        clustering = DBSCAN(eps=0.3, min_samples=10).fit(xy_points)
+        labels = clustering.labels_
+        unique_labels = set(labels)
+        found = False
+        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)
+            if 1.5 < width < 2.1 and 0.7 < height < 1.3:
+                cloud_msg = self.xyz_array_to_pointcloud2(cluster, msg.header)
+                self.publisher.publish(cloud_msg)
+
+                # 用PCA直接找矩形四角
+                corners = fit_rectangle_pca(cluster)
+                from itertools import permutations
+                best_score = float('inf')
+                best_rect = None
+                for order in permutations(range(4)):
+                    ordered = [corners[i] for i in order]
+                    score = rectangle_score(np.array(ordered))
+                    if score < best_score:
+                        best_score = score
+                        best_rect = ordered
+                rect_lines = best_rect
+                if rect_lines is None or len(rect_lines) < 4:
+                    self.get_logger().info('未找到合适矩形')
+                    continue
+
+                # 发布最新的矩形
+                marker = Marker()
+                marker.header = msg.header
+                marker.ns = "basketball_frame"
+                marker.id = 0
+                marker.type = Marker.LINE_LIST
+                marker.action = Marker.ADD
+                marker.scale.x = 0.08
+                marker.color.r = 0.0
+                marker.color.g = 1.0
+                marker.color.b = 0.0
+                marker.color.a = 1.0
+                marker.points = []
+                from geometry_msgs.msg import Point
+                for i in range(4):
+                    p1 = rect_lines[i]
+                    p2 = rect_lines[(i+1)%4]
+                    pt1 = Point(x=float(p1[0]), y=float(p1[1]), z=float(p1[2]))
+                    pt2 = Point(x=float(p2[0]), y=float(p2[1]), z=float(p2[2]))
+                    marker.points.append(pt1)
+                    marker.points.append(pt2)
+                self.marker_pub.publish(marker)
+
+                # 分别发布4条最优边线
+                for i in range(4):
+                    edge_marker = Marker()
+                    edge_marker.header = msg.header
+                    edge_marker.ns = "basketball_frame_edges"
+                    edge_marker.id = i
+                    edge_marker.type = Marker.LINE_STRIP
+                    edge_marker.action = Marker.ADD
+                    edge_marker.scale.x = 0.12
+                    colors = [
+                        (1.0, 0.0, 0.0),
+                        (0.0, 1.0, 0.0),
+                        (0.0, 0.0, 1.0),
+                        (1.0, 1.0, 0.0)
+                    ]
+                    edge_marker.color.r = colors[i][0]
+                    edge_marker.color.g = colors[i][1]
+                    edge_marker.color.b = colors[i][2]
+                    edge_marker.color.a = 1.0
+                    pt1 = Point(x=float(rect_lines[i][0]), y=float(rect_lines[i][1]), z=float(rect_lines[i][2]))
+                    pt2 = Point(x=float(rect_lines[(i+1)%4][0]), y=float(rect_lines[(i+1)%4][1]), z=float(rect_lines[(i+1)%4][2]))
+                    edge_marker.points = [pt1, pt2]
+                    self.marker_pub.publish(edge_marker)
+
+                # 中心点滑动平均
+                center = np.mean(np.array(rect_lines), axis=0)
+                self.center_buffer.append(center)
+                if len(self.center_buffer) > self.center_buffer_size:
+                    self.center_buffer.pop(0)
+                stable_center = np.mean(self.center_buffer, axis=0)
+
+                # 发布中心点Marker
+                center_marker = Marker()
+                center_marker.header = msg.header
+                center_marker.ns = "basketball_frame"
+                center_marker.id = 1
+                center_marker.type = Marker.SPHERE
+                center_marker.action = Marker.ADD
+                center_marker.scale.x = 0.15
+                center_marker.scale.y = 0.15
+                center_marker.scale.z = 0.15
+                center_marker.color.r = 1.0
+                center_marker.color.g = 0.0
+                center_marker.color.b = 0.0
+                center_marker.color.a = 1.0
+                center_marker.pose.position.x = float(stable_center[0])
+                center_marker.pose.position.y = float(stable_center[1])
+                center_marker.pose.position.z = float(stable_center[2])
+                center_marker.pose.orientation.x = 0.0
+                center_marker.pose.orientation.y = 0.0
+                center_marker.pose.orientation.z = 0.0
+                center_marker.pose.orientation.w = 1.0
+                self.marker_pub.publish(center_marker)
+
+                # 计算法向量（篮板主方向，PCA最小特征值方向）
+                pts = np.array(rect_lines)
+                mean = np.mean(pts, axis=0)
+                cov = np.cov(pts.T)
+                eigvals, eigvecs = np.linalg.eigh(cov)
+                order = np.argsort(eigvals)[::-1]
+                normal_vec = eigvecs[:, order[2]]
+                normal_vec = normal_vec / np.linalg.norm(normal_vec)
+                # 向内侧偏移30cm
+                offset_point = stable_center + 0.3 * normal_vec
+
+                # 发布偏移点Marker
+                offset_marker = Marker()
+                offset_marker.header = msg.header
+                offset_marker.ns = "basketball_frame"
+                offset_marker.id = 2
+                offset_marker.type = Marker.SPHERE
+                offset_marker.action = Marker.ADD
+                offset_marker.scale.x = 0.12
+                offset_marker.scale.y = 0.12
+                offset_marker.scale.z = 0.12
+                offset_marker.color.r = 0.0
+                offset_marker.color.g = 0.0
+                offset_marker.color.b = 1.0
+                offset_marker.color.a = 1.0
+                offset_marker.pose.position.x = float(offset_point[0])
+                offset_marker.pose.position.y = float(offset_point[1])
+                offset_marker.pose.position.z = float(offset_point[2])
+                offset_marker.pose.orientation.x = 0.0
+                offset_marker.pose.orientation.y = 0.0
+                offset_marker.pose.orientation.z = 0.0
+                offset_marker.pose.orientation.w = 1.0
+                self.marker_pub.publish(offset_marker)
+
+                found = True
+                break
+        if not found:
+            self.get_logger().info('本帧未找到矩形')
+
+    def pointcloud2_to_xyz(self, cloud_msg):
+        fmt = 'ffff'
+        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):
+        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/pcd2pgm.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/save_pcd.py

@@ -0,0 +1,55 @@
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import PointCloud2
+import numpy as np
+import open3d as o3d
+import time
+
+class PointCloudSaver(Node):
+    def __init__(self):
+        super().__init__('pcd_saver')
+        self.subscription = self.create_subscription(
+            PointCloud2,
+            '/livox/lidar_filtered',
+            self.listener_callback,
+            10)
+        self.point_clouds = []
+        self.start_time = time.time()
+        self.timer = self.create_timer(3.0, self.save_and_exit)
+        self.saving = False
+
+    def listener_callback(self, msg):
+        if not self.saving:
+            pc = self.pointcloud2_to_xyz_array(msg)
+            if pc is not None:
+                self.point_clouds.append(pc)
+
+    def pointcloud2_to_xyz_array(self, cloud_msg):
+        # 仅支持 x, y, z 均为 float32 且无 padding 的点云
+        import struct
+        points = []
+        point_step = cloud_msg.point_step
+        for i in range(cloud_msg.width * cloud_msg.height):
+            offset = i * point_step
+            x, y, z = struct.unpack_from('fff', cloud_msg.data, offset)
+            points.append([x, y, z])
+        return np.array(points)
+
+    def save_and_exit(self):
+        if not self.saving:
+            self.saving = True
+            if self.point_clouds:
+                all_points = np.vstack(self.point_clouds)
+                pcd = o3d.geometry.PointCloud()
+                pcd.points = o3d.utility.Vector3dVector(all_points)
+                o3d.io.write_point_cloud("output.pcd", pcd)
+                self.get_logger().info("Saved output.pcd")
+            rclpy.shutdown()
+
+def main(args=None):
+    rclpy.init(args=args)
+    saver = PointCloudSaver()
+    rclpy.spin(saver)
+
+if __name__ == '__main__':
+    main()

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()

src/rm_driver/rm_serial_driver/script/R2_Serial.py

@@ -18,6 +18,8 @@ class R2SerialNode(Node):
         self.distance_port = self.get_parameter('distance_port').get_parameter_value().string_value
         self.baud_rate = self.get_parameter('baud_rate').get_parameter_value().integer_value
         
+        # 创建定时器
+        # self.timer = self.create_timer(0.01, self.send_data)  # 每100ms发送一次数据
         # 初始化串口
         try:
             self.yaw_serial = serial.Serial(self.yaw_port, self.baud_rate, timeout=1)

src/rm_driver/rm_serial_driver/script/pub_goal.py

@@ -0,0 +1,36 @@
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import PointStamped
+from rm_msgs.msg import MoveGoal
+
+class ClickedGoalPublisher(Node):
+    def __init__(self):
+        super().__init__('clicked_goal_publisher')
+        self.subscription = self.create_subscription(
+            PointStamped,
+            '/clicked_point',
+            self.clicked_callback,
+            10
+        )
+        self.publisher = self.create_publisher(MoveGoal, '/move_goal', 10)
+
+    def clicked_callback(self, msg):
+        goal = MoveGoal()
+        goal.x = msg.point.x
+        goal.y = msg.point.y
+        goal.angle = 0.0
+        goal.max_speed = 0.0
+        goal.tolerance = 0.1
+        goal.rotor = False
+        self.publisher.publish(goal)
+        self.get_logger().info(f'发布目标: x={goal.x:.2f}, y={goal.y:.2f}')
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = ClickedGoalPublisher()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/rm_driver/rm_serial_driver/script/receive.py

@@ -36,7 +36,7 @@ class SerialReceiver(Node):
         self.get_logger().info(f"打开串口 {SERIAL_PORT}，波特率 {BAUDRATE}")
         
         # 创建定时器用于读取串口数据
-        self.timer = self.create_timer(0.01, self.read_serial_data)  # 10ms
+        self.timer = self.create_timer(0.001, self.read_serial_data)  # 10ms
         
         self.buffer = bytearray()
         

src/rm_localization/icp_registration/include/icp_registration/icp_registration.hpp

@@ -50,7 +50,7 @@ private:
       initial_pose_sub_;
   rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr
       pointcloud_sub_;
-  // rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::TimerBase::SharedPtr timer_;
   std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
   std::shared_ptr<tf2_ros::TransformListener> tf_listener_;

src/rm_nav_bringup/config/reality/measurement_params_real.yaml

@@ -1,3 +1,3 @@
 base_link2livox_frame:
-  xyz: "\"0.246 -0.135 0.397\""
+  xyz: "\"0.251 -0.1285 0.397\""
   rpy: "\"0.0  0.0  0.0\""