class ImageConverter:
def __init__(self):
rospy.init_node(ROS_NODE_NAME, anonymous=True)
self.vel_pub = rospy.Publisher(VEL_TOPIC, Twist, queue_size=10)
self.buzzer_pub = rospy.Publisher(BUZZER_TOPIC, UInt16, queue_size=10)
self.image_sub = rospy.Subscriber(IMAGE_TOPIC, Image, self.callback)
self.buzzer_control = BuzzerControl()
global NORMAL_LINE_AREA, cross_count, nav_mode
NORMAL_LINE_AREA = None
cross_count = 0
nav_mode = 0
rospy.loginfo("初始模式：黑线循迹模式")
self.buzzer_pub.publish(UInt16(data=200))
def image_preprocess(self, image):
"""图像预处理：ROI 裁剪→灰度化→高斯滤波→自适应二值化→形态学开运算"""
roi_image = image[ROI_Y1:ROI_Y2, ROI_X1:ROI_X2]
gray_image = cv2.cvtColor(roi_image, cv2.COLOR_BGR2GRAY)
blur_image = cv2.GaussianBlur(gray_image, (5, 5), 0)
thresh_image = cv2.adaptiveThreshold(
blur_image, 255, cv2.ADAPTIVE_THRESH_GAUSSIAN_C,
cv2.THRESH_BINARY_INV, ADAPTIVE_THRESH_BLOCK_SIZE, ADAPTIVE_THRESH_C
)
kernel = np.ones((3, 3), np.uint8)
opening_image = cv2.morphologyEx(thresh_image, cv2.MORPH_OPEN, kernel)
return roi_image, opening_image
def black_line_detect(self, binary_image):
"""黑线检测：轮廓提取→最大轮廓筛选→质心计算→偏离量求解"""
contours, _ = cv2.findContours(binary_image, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
deviation = 0
has_line = False
if len(contours) > 0:
max_contour = max(contours, key=cv2.contourArea)
M = cv2.moments(max_contour)
if M["m00"] != 0:
cx = int(M["m10"] / M["m00"])
cy = int(M["m01"] / M["m00"])
image_center_x = binary_image.shape[1] // 2
deviation = image_center_x - cx
has_line = True
cv2.circle(binary_image, (cx, cy), 5, (255, 0, 0), -1)
cv2.putText(binary_image, f"Deviation: {deviation}", (10, 30),
cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 2)
return has_line, deviation, binary_image
def cross_detect(self, binary_image):
"""路口检测：通过轮廓面积判断"""
global NORMAL_LINE_AREA, cross_count
line_area = np.sum(binary_image == 255)
if NORMAL_LINE_AREA is None:
if cross_count < 3:
cross_count += 1
NORMAL_LINE_AREA = line_area
else:
NORMAL_LINE_AREA = (NORMAL_LINE_AREA * 2 + line_area) / 3
return False
if line_area > CROSS_AREA_THRESHOLD and line_area > NORMAL_LINE_AREA * 1.5:
return True
return False
def infrared_light_detect(self, image):
"""红外激光笔检测：HSV 颜色分割→滤波→轮廓提取→光点定位"""
hsv_image = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv_image, LOWER_RED, UPPER_RED)
mask = cv2.medianBlur(mask, 7)
contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
laser_center = None
if len(contours) > 0:
for cnt in contours:
area = cv2.contourArea(cnt)
if area > LASER_MIN_AREA:
rect = cv2.minAreaRect(cnt)
cx, cy = int(rect[0][0]), int(rect[0][1])
laser_center = (cx, cy)
cv2.circle(image, (cx, cy), 5, (0, 255, 0), -1)
cv2.putText(image, "Laser Found", (10, 30),
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
break
return laser_center, image, mask
def callback(self, data):
"""主回调函数：接收图像数据，根据导航模式执行对应逻辑"""
global cross_count, nav_mode
try:
cv_image = bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
rospy.logerr("图像转换失败：%s", str(e))
return
if nav_mode == 0:
roi_image, binary_image = self.image_preprocess(cv_image)
is_cross = self.cross_detect(binary_image)
if is_cross and cross_count <= MAX_CROSS_COUNT:
cross_count += 1
rospy.loginfo(f"检测到路口，当前计数：{cross_count}/{MAX_CROSS_COUNT}")
self.buzzer_pub.publish(UInt16(data=300))
has_line, deviation, binary_image = self.black_line_detect(binary_image)
if has_line:
if cross_count <= MAX_CROSS_COUNT:
if deviation < -DEVIATION_THRESHOLD:
self.vel_pub.publish(msg_left_decelerate)
elif deviation > DEVIATION_THRESHOLD:
self.vel_pub.publish(msg_right_decelerate)
else:
self.vel_pub.publish(msg_forward_decelerate)
else:
if deviation < -DEVIATION_THRESHOLD:
self.vel_pub.publish(msg_left)
elif deviation > DEVIATION_THRESHOLD:
self.vel_pub.publish(msg_right)
else:
self.vel_pub.publish(msg_forward)
if cross_count > MAX_CROSS_COUNT:
nav_mode = 1
rospy.loginfo("切换至激光跟随模式")
self.buzzer_pub.publish(UInt16(data=500))
else:
self.vel_pub.publish(msg_stop)
rospy.logwarn("未检测到黑线，已停车")
cv2.imshow("Black Line Tracking View", roi_image)
cv2.imshow("Binary Image", binary_image)
elif nav_mode == 1:
laser_center, laser_view_image, mask_image = self.infrared_light_detect(cv_image)
if laser_center is not None:
cx, cy = laser_center
image_center_x = cv_image.shape[1] // 2
laser_deviation = cx - image_center_x
if laser_deviation < -LASER_DEVIATION_THRESHOLD:
self.vel_pub.publish(msg_left)
elif laser_deviation > LASER_DEVIATION_THRESHOLD:
self.vel_pub.publish(msg_right)
else:
self.vel_pub.publish(msg_forward)
else:
self.vel_pub.publish(msg_stop)
rospy.logwarn("未检测到激光点，已停车")
cv2.imshow("Laser Following View", laser_view_image)
cv2.imshow("Laser Mask", mask_image)
key = cv2.waitKey(1) & 0xFF
if key == 27:
rospy.signal_shutdown("用户按下 ESC 键，退出程序")
cv2.destroyAllWindows()
