From 21d105a0fc09d992fb214ca47ff6cfd4d0fb0f7b Mon Sep 17 00:00:00 2001 From: robofish <1683502971@qq.com> Date: Fri, 11 Jul 2025 22:53:01 +0800 Subject: [PATCH] =?UTF-8?q?=E7=82=B9=E5=87=86=E4=BA=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/rc_lidar/juxing.py | 92 +++++++++++++++++++++++++++++++++--------- 1 file changed, 73 insertions(+), 19 deletions(-) diff --git a/src/rc_lidar/juxing.py b/src/rc_lidar/juxing.py index 67a2b8f..d573c7b 100644 --- a/src/rc_lidar/juxing.py +++ b/src/rc_lidar/juxing.py @@ -7,6 +7,7 @@ import sensor_msgs_py.point_cloud2 as pc2 from visualization_msgs.msg import Marker import std_msgs.msg import geometry_msgs.msg +import collections class RectangleDetector(Node): def __init__(self): @@ -20,11 +21,37 @@ class RectangleDetector(Node): self.marker_pub = self.create_publisher(Marker, '/rectangle_marker', 10) # 可选：发布原始点云 self.cloud_pub = self.create_publisher(PointCloud2, '/rectangle_cloud', 10) + self.history = collections.deque(maxlen=5) + self.last_bbox = None + self.last_time = None # 目标矩形尺寸（单位：米） self.length = 1.8 self.width = 1.05 self.thickness = 0.04 + def filter_points(self, points): + # 统计滤波：去除离群点 + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(points) + cl, ind = pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0) + filtered = np.asarray(cl.points) + # 半径滤波 + cl2, ind2 = cl.remove_radius_outlier(nb_points=10, radius=0.05) + return np.asarray(cl2.points) + + def smooth_bbox(self, bbox): + # 保存历史中心和尺寸 + center = bbox.center + extent = bbox.extent + self.history.append((center, extent)) + # 计算加权平均 + centers = np.array([h[0] for h in self.history]) + extents = np.array([h[1] for h in self.history]) + smooth_center = np.mean(centers, axis=0) + smooth_extent = np.mean(extents, axis=0) + bbox.center = smooth_center + bbox.extent = smooth_extent + return bbox def publish_marker(self, bbox): @@ -33,17 +60,37 @@ class RectangleDetector(Node): marker.header.stamp = self.get_clock().now().to_msg() marker.ns = "rectangle_corners" marker.id = 0 - marker.type = Marker.SPHERE_LIST # 或 Marker.POINTS + marker.type = Marker.SPHERE_LIST marker.action = Marker.ADD - # 获取8个旋转后的角点 corners = np.asarray(bbox.get_box_points()) + center = bbox.center + + # 展示角点 for c in corners: pt = geometry_msgs.msg.Point() pt.x, pt.y, pt.z = c marker.points.append(pt) - marker.scale.x = 0.08 # 球半径或点大小 + # 展示中心点 + center_pt = geometry_msgs.msg.Point() + center_pt.x, center_pt.y, center_pt.z = center + marker.points.append(center_pt) + + # 沿中心点到原点方向，向外移动10cm + origin = np.array([0.0, 0.0, 0.0]) + direction = origin - center + norm = np.linalg.norm(direction) + if norm > 0: + move_vec = direction / norm * 0.46 + moved_pt = center + move_vec + else: + moved_pt = center + out_pt = geometry_msgs.msg.Point() + out_pt.x, out_pt.y, out_pt.z = moved_pt + marker.points.append(out_pt) + + marker.scale.x = 0.08 marker.scale.y = 0.08 marker.scale.z = 0.08 marker.color.r = 0.0 @@ -53,7 +100,6 @@ class RectangleDetector(Node): marker.lifetime.sec = 1 marker.lifetime.nanosec = 0 self.marker_pub.publish(marker) - # ...existing code... # ...existing code... def pointcloud_callback(self, msg): @@ -68,39 +114,47 @@ class RectangleDetector(Node): return points = np.array(points_list) - # 发布原始点云到RViz + # 点云预处理 + # points = self.filter_points(points) + if len(points) == 0: + self.get_logger().info('滤波后点云为空') + return + self.cloud_pub.publish(msg) - # 转换为Open3D点云对象 pcd = o3d.geometry.PointCloud() pcd.points = o3d.utility.Vector3dVector(points) - - # 添加体素滤波，滤波分辨率可调 - voxel_size = 0.03 # 3cm分辨率 + voxel_size = 0.03 pcd = pcd.voxel_down_sample(voxel_size=voxel_size) - # 使用RANSAC拟合矩形框（近似为平面+边界筛选） plane_model, inliers = pcd.segment_plane( distance_threshold=0.02, ransac_n=3, num_iterations=2000 ) inlier_cloud = pcd.select_by_index(inliers) - # bbox = inlier_cloud.get_axis_aligned_bounding_box() - bbox = inlier_cloud.get_oriented_bounding_box() # 修改为贴合实际方向 + bbox = inlier_cloud.get_oriented_bounding_box() extent = bbox.extent center = bbox.center - self.get_logger().info(f'滤波后bbox中心: {center}, 尺寸: {extent}') - dims = sorted(extent) - if (abs(dims[0] - self.thickness) < 1.0 and - abs(dims[1] - self.width) < 1.0 and - abs(dims[2] - self.length) < 1.0): - self.get_logger().info(f'检测到目标矩形框，位置: {center}, 尺寸: {extent}') + # 检测阈值收紧 + if (abs(dims[0] - self.thickness) < 0.1 and + abs(dims[1] - self.width) < 0.2 and + abs(dims[2] - self.length) < 0.2): + bbox = self.smooth_bbox(bbox) + self.get_logger().info(f'检测到目标矩形框，位置: {bbox.center}, 尺寸: {bbox.extent}') self.publish_marker(bbox) + self.last_bbox = bbox + self.last_time = self.get_clock().now() else: - self.get_logger().info('未检测到目标矩形框') + # 若上一帧有结果且时间间隔<0.5s，则继续发布上一帧 + now = self.get_clock().now() + if self.last_bbox and self.last_time and (now - self.last_time).nanoseconds < 5e8: + self.get_logger().info('本帧未检测到，使用上一帧结果') + self.publish_marker(self.last_bbox) + else: + self.get_logger().info('未检测到目标矩形框') def main(args=None): rclpy.init(args=args)