267 lines
10 KiB
Python
Executable File
267 lines
10 KiB
Python
Executable File
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() |