import rclpy from rclpy.node import Node from cv_bridge import CvBridge from sensor_msgs.msg import Image from std_msgs.msg import Float32 import cv2 import numpy as np from message_filters import ApproximateTimeSynchronizer, Subscriber from rclpy.qos import QoSProfile, ReliabilityPolicy class BirdseyeNode(Node): def __init__(self): super().__init__('birdseye_node') self.bridge = CvBridge() # 左右鱼眼订阅 self.left_sub = Subscriber(self, Image, '/image_left') self.right_sub = Subscriber(self, Image, '/image_right') self.ts = ApproximateTimeSynchronizer( [self.left_sub, self.right_sub], queue_size=10, slop=0.5 ) self.ts.registerCallback(self.callback) qos = QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT) self.proc_pub_left = self.create_publisher(Image, '/image_proc_left', 10) self.proc_pub_right = self.create_publisher(Image, '/image_proc_right', 10) self.trans_pub_left = self.create_publisher(Image, '/image_tran_left', 10) self.trans_pub_right = self.create_publisher(Image, '/image_tran_right', 10) self.line_error_pub = self.create_publisher(Float32, '/line_error', qos) self.last_slope = 0.0 # 前置RGB订阅 self.rgb_sub = self.create_subscription(Image, '/image_rgb', self.rgb_callback, 10) self.proc_pub_rgb = self.create_publisher(Image, '/image_proc_rgb', 10) self.line_distance_pub = self.create_publisher(Float32, '/line_distance', qos) # 相机参数 fx = 215.0699086206705 fy = 215.0699086206705 cx = 250.6595680010422 cy = 197.9387845612447 self.K = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]]) self.D = np.array([-2.803862613372903e-04, -7.223158724979862e-06, 1.437534138630982e-08, 0.0]) self.dim = (500, 400) new_K = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify( self.K, self.D, self.dim, np.eye(3), balance=0.24 ) new_K[0, 2], new_K[1, 2] = self.dim[0] / 2, self.dim[1] / 2 self.map1_left, self.map2_left = cv2.fisheye.initUndistortRectifyMap( self.K, self.D, np.eye(3), new_K, self.dim, cv2.CV_16SC2 ) self.map1_right, self.map2_right = cv2.fisheye.initUndistortRectifyMap( self.K, self.D, np.eye(3), new_K, self.dim, cv2.CV_16SC2 ) self.get_logger().info("鱼眼去畸变节点启动") self.filtered_slope_left = None self.filtered_slope_right = None self.alpha = 1.0 self.jump_threshold = 0.05 self.line_error = 0.0 # 保存 line_error 供 distance 调整使用 def adaptive_ema(self, new_val, prev_filtered): if prev_filtered is None: return new_val if abs(new_val - prev_filtered) > self.jump_threshold: return new_val return self.alpha * new_val + (1 - self.alpha) * prev_filtered def detect_slope(self, img, side='left', roi_h=180, roi_w=200, y_start=None): """黄线检测 + 底部边界斜率计算""" h, w = img.shape[:2] # ROI纵向范围 if y_start is None: y1 = h - roi_h else: y1 = y_start y2 = y1 + roi_h # ROI横向范围 x_center = w // 2 x1, x2 = x_center - roi_w // 2, x_center + roi_w // 2 roi = img[y1:y2, x1:x2] x_offset, y_offset = x1, y1 hsv = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV) # 黄色范围 if side == 'rgb': lower_yellow = np.array([20, 80, 60]) upper_yellow = np.array([45, 255, 255]) else: lower_yellow = np.array([25, 100, 60]) upper_yellow = np.array([45, 255, 255]) mask = cv2.inRange(hsv, lower_yellow, upper_yellow) kernel = np.ones((5, 5), np.uint8) mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel) mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel) _, contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) if not contours: return None, img.copy(), None largest = max(contours, key=cv2.contourArea) if cv2.contourArea(largest) < 30: return None, img.copy(), None contour_shifted = largest + np.array([[x_offset, y_offset]]) all_pts = contour_shifted.reshape(-1, 2) hsv_full = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) black_mask = cv2.inRange(hsv_full, np.array([0, 0, 0]), np.array([180, 60, 160])) bottom_edge_pts = [] x_min, x_max = all_pts[:, 0].min(), all_pts[:, 0].max() for x in range(x_min, x_max + 1): col_pts = all_pts[all_pts[:, 0] == x] if len(col_pts) > 0: y_bottom = col_pts[:, 1].max() check_y = min(y_bottom + 1, h - 1) if black_mask[check_y, x] > 0: bottom_edge_pts.append([x, y_bottom]) bottom_edge_pts = np.array(bottom_edge_pts) if len(bottom_edge_pts) < 2: return None, img.copy(), None m, b = np.polyfit(bottom_edge_pts[:, 0], bottom_edge_pts[:, 1], 1) m_corrected = -m img_with_line = img.copy() x1_line, x2_line = bottom_edge_pts[:, 0].min(), bottom_edge_pts[:, 0].max() y1_line, y2_line = m * x1_line + b, m * x2_line + b cv2.line(img_with_line, (int(x1_line), int(y1_line)), (int(x2_line), int(y2_line)), (0, 0, 255), 2) cv2.drawContours(img_with_line, [contour_shifted], -1, (255, 0, 0), 2) for px, py in bottom_edge_pts: cv2.circle(img_with_line, (int(px), int(py)), 3, (255, 0, 255), -1) return m_corrected, img_with_line, bottom_edge_pts def rgb_callback(self, msg): """RGB前置摄像头处理""" try: img = self.bridge.imgmsg_to_cv2(msg, 'bgr8') h, w = img.shape[:2] y_start = h - 180 roi_h = 180 roi_w = w slope, proc_img, _ = self.detect_slope( img, side='rgb', roi_h=roi_h, roi_w=roi_w, y_start=y_start ) if slope == 0.0 or slope is None: self.get_logger().info(f"[RGB] 使用上一帧斜率: {self.last_slope:.4f}") slope = self.last_slope self.last_slope = slope self.proc_pub_rgb.publish(self.bridge.cv2_to_imgmsg(proc_img, 'bgr8')) self.rgb_slope = slope self.get_logger().info(f"[RGB] 计算斜率: {slope:.4f}") except Exception as e: self.get_logger().error(f"[RGB] 处理失败: {e}") self.rgb_slope = 0.0 def callback(self, left_msg, right_msg): """左右鱼眼主回调""" try: left_img = self.bridge.imgmsg_to_cv2(left_msg, 'bgr8') right_img = self.bridge.imgmsg_to_cv2(right_msg, 'bgr8') except Exception as e: self.get_logger().error(f"[步骤1] 转换图像失败: {e}") return try: left_undistort = cv2.remap( left_img, self.map1_left, self.map2_left, cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT ) right_undistort = cv2.remap( right_img, self.map1_right, self.map2_right, cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT ) right_undistort = cv2.resize(right_undistort, (left_undistort.shape[1], left_undistort.shape[0])) self.trans_pub_left.publish(self.bridge.cv2_to_imgmsg(left_undistort, 'bgr8')) self.trans_pub_right.publish(self.bridge.cv2_to_imgmsg(right_undistort, 'bgr8')) except Exception as e: self.get_logger().error(f"[步骤2] 去畸变或发布失败: {e}") return try: h, w = left_undistort.shape[:2] roi_w, roi_h = 80, 240 slope_left, left_proc_img, left_pts = self.detect_slope(left_undistort, side='left', roi_h=roi_h, roi_w=roi_w) slope_right, right_proc_img, right_pts = self.detect_slope(right_undistort, side='right', roi_h=roi_h, roi_w=roi_w) distance_left = np.mean(left_pts[:, 1]) if left_pts is not None else 0.0 distance_right = np.mean(right_pts[:, 1]) if right_pts is not None else 0.0 line_distance = distance_left - distance_right # 如果 line_error 和 line_distance 同号，则 distance * 1.5 if (self.line_error > 0 and line_distance > 0) or (self.line_error < 0 and line_distance < 0): line_distance *= 1.5 if left_pts is not None: for px, py in left_pts: cv2.line(left_proc_img, (px, py), (px, int(h)), (0, 0, 255), 1) if right_pts is not None: for px, py in right_pts: cv2.line(right_proc_img, (px, py), (px, int(h)), (0, 0, 255), 1) self.line_distance_pub.publish(Float32(data=float(line_distance))) self.get_logger().info(f"[FishEye] 左右底部距离: {distance_left:.2f}, {distance_right:.2f}, line_distance: {line_distance:.2f}") self.proc_pub_left.publish(self.bridge.cv2_to_imgmsg(left_proc_img, 'bgr8')) self.proc_pub_right.publish(self.bridge.cv2_to_imgmsg(right_proc_img, 'bgr8')) except Exception as e: self.get_logger().error(f"[步骤3] 鱼眼处理失败: {e}") return try: rgb_slope = getattr(self, 'rgb_slope', 0.0) if rgb_slope > 0: self.line_error = 6.0 elif rgb_slope < 0: self.line_error = -10.0 else: self.line_error = 0.0 self.get_logger().info(f"RGB斜率(line_error): {self.line_error:.4f}") self.line_error_pub.publish(Float32(data=float(self.line_error))) except Exception as e: self.get_logger().error(f"[步骤4] 发布line_error失败: {e}") return def main(args=None): rclpy.init(args=args) node = BirdseyeNode() try: rclpy.spin(node) except KeyboardInterrupt: pass node.destroy_node() rclpy.shutdown() if __name__ == '__main__': main()