#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
智能车竞赛全自动最终版（修复未定义变量）
流程（严格顺序）：
等待拆挡板 -> 发车计时（前轮过线）-> 巡线 + 避障（贯穿）-> 斑马线停 3s -> 变道 -> A/B 识别 -> 停车（黄线）-> 自动付费 -> 结束
摄像头索引 = 2（与你提供的一致）
"""
import os
import time
import datetime
import cv2
import numpy as np
import pigpio
import onnxruntime
from PIL import Image, ImageDraw, ImageFont
import sys
# -----------------------------
# 全局配置（严格沿用你提供的参数）
# -----------------------------
CAM_INDEX = 2
W, H = 640, 480
FPS_TARGET = 20
DATA_SAVE_DIR = "/home/pi/smartcar_data"
os.makedirs(DATA_SAVE_DIR, exist_ok=True)
SAVE_INTERVAL = 30 # 每隔30秒拍照存数据
# 硬件（舵机/电机）参数
MOTOR_PIN = 13
NEUTRAL_PULSE = 1500
FORWARD_BASE = 1380
FORWARD_LOW = 1390
BACKWARD_PULSE = 1500
SERVO_PIN = 12
SERVO_MIN_ANGLE = 70
SERVO_MAX_ANGLE = 110
SERVO_MID_ANGLE = 90
SERVO_MIN_PULSE = 500
SERVO_MAX_PULSE = 2500
# PID 与控制参数
KP = 0.09
KD = 0.07
DEAD_ZONE = 5.0
TURN_DECAY = 0.85
MAX_TURN_DURATION = 8
TRACK_LOST_THRESH = 5
RECOVER_SPEED = FORWARD_LOW - 60
SINGLE_LINE_OFFSET = 80
# 白线 / Hough 参数
WHITE_THRESHOLD = 170
MIN_LINE_LENGTH = 20
MAX_LINE_GAP = 8
ANGLE_RANGE = (0, 55)
ROI_RATIO = 2 / 3
# 蓝色锥桶（避障）参数（沿用）
OBSTACLE_COLOR_LOWER = np.array([97, 154, 60])
OBSTACLE_COLOR_UPPER = np.array([117, 255, 255])
OBSTACLE_AREA_MIN = 600
OBSTACLE_AREA_MAX = 20000
ASPECT_RATIO_RANGE = (0.5, 1.4)
EXTENT_THRESHOLD = 0.95
# 斑马线参数（只检测下半 ROI）
ZEBRA_RATIO_THRESH = 0.3
CROSSWALK_STOP_DURATION = 3
CROSSWALK_SPEECH = "已到达斑马线，停车3秒后继续前行"
# 变道 / 箭头识别参数（保持原路径）
ARROW_MODEL_PATH = "/home/pi/arrow_best.onnx"
ARROW_CLASS_NAMES = ["arrow_left", "arrow_right"]
SIGN_COLOR_LOWER = np.array([100, 80, 50])
SIGN_COLOR_UPPER = np.array([130, 255, 255])
ARROW_BINARY_THRESH = 160
SIGN_AREA_MIN = 200 # <-- **已补定义**（之前缺失）
CONF_THRESHOLD = 0.4
REQUIRED_CONSECUTIVE = 3
# A/B 识别
AB_MODEL_PATH = "/home/pi/ab_best.onnx"
AB_CLASS_NAMES = ["A", "B"]
# 黄线停车
YELLOW_COLOR_LOWER = np.array([20, 100, 100])
YELLOW_COLOR_UPPER = np.array([30, 255, 255])
YELLOW_LINE_MIN_LENGTH = 200
# 停车区分左右
PARK_AREA_LEFT = (0, W//2)
PARK_AREA_RIGHT = (W//2, W)
# 付费（自动模拟成功）
PAY_DURATION = 30
PAY_AUTO_SUCCESS_AFTER = 5 # 自动模拟 5s 后付费成功（可改为集成真实接口）
PAY_SCORE = 5
PAY_PROMPT = "请在30秒内扫码支付0.01元，自动模拟支付成功"
# 字体
FONT_PATH = "/usr/share/fonts/truetype/wqy/wqy-zenhei.ttc"
FONT_SIZE = 24
# 状态机（严格顺序）
STATE_WAIT_START = "WAIT_START"
STATE_START = "START"
STATE_TRACK = "TRACK" # 巡线始终运行
STATE_CROSSWALK = "CROSSWALK"
STATE_LANE_CHANGE = "LANE_CHANGE"
STATE_AB_RECOG = "AB_RECOG"
STATE_PARK = "PARK"
STATE_PAY = "PAY"
STATE_FINISH = "FINISH"
# -----------------------------
# 硬件控制类（保留并简化接口）
# -----------------------------
class HardwareController:
def __init__(self):
self.pi = pigpio.pi()
if not self.pi.connected:
raise RuntimeError("请启动 pigpio 服务： sudo pigpiod")
self.last_error = 0.0
self.current_angle = SERVO_MID_ANGLE
self.track_lost_count = 0
self.turn_duration = 0
self.last_turn_direction = 0
# unlock motor
self.pi.set_servo_pulsewidth(MOTOR_PIN, NEUTRAL_PULSE)
time.sleep(1.0)
self.pi.set_servo_pulsewidth(MOTOR_PIN, NEUTRAL_PULSE)
time.sleep(0.5)
self.set_servo_angle(SERVO_MID_ANGLE)
print("[HW] 初始化完成")
def angle_to_pulse(self, angle):
angle = np.clip(angle, SERVO_MIN_ANGLE, SERVO_MAX_ANGLE)
return int(SERVO_MIN_PULSE + (angle / 180.0) * (SERVO_MAX_PULSE - SERVO_MIN_PULSE))
def set_servo_angle(self, angle):
# 平滑舵机变化，防抖
smoothed = 0.6 * angle + 0.4 * self.current_angle
pulse = self.angle_to_pulse(smoothed)
self.pi.set_servo_pulsewidth(SERVO_PIN, pulse)
self.current_angle = smoothed
def set_motor_speed(self, pulse):
pulse = int(np.clip(pulse, 1000, 2000))
self.pi.set_servo_pulsewidth(MOTOR_PIN, pulse)
def pid_control(self, error):
# 使用你给的完全反转误差映射逻辑
error = -error
current_direction = 0
if error > DEAD_ZONE:
current_direction = 2
elif error < -DEAD_ZONE:
current_direction = 1
if current_direction != self.last_turn_direction:
self.turn_duration = 0
self.last_turn_direction = current_direction
else:
self.turn_duration += 1
if self.turn_duration >= MAX_TURN_DURATION:
error *= TURN_DECAY
if abs(error) < DEAD_ZONE:
error = 0.0
self.turn_duration = 0
p = KP * error
d = KD * (error - self.last_error)
self.last_error = error
total_offset = p + d
target_angle = SERVO_MID_ANGLE + total_offset
self.set_servo_angle(target_angle)
self.set_motor_speed(FORWARD_BASE)
def stop(self):
self.set_motor_speed(NEUTRAL_PULSE)
def cleanup(self):
try:
self.stop()
self.set_servo_angle(SERVO_MID_ANGLE)
self.pi.stop()
except:
pass
print("[HW] 清理完成")
# -----------------------------
# 图像与模型工具函数（保留原逻辑）
# -----------------------------
def draw_chinese(frame, text, pos=(10, 30), color=(255,255,0)):
img_rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
pil = Image.fromarray(img_rgb)
draw = ImageDraw.Draw(pil)
try:
font = ImageFont.truetype(FONT_PATH, FONT_SIZE)
except:
font = ImageFont.load_default()
draw.text(pos, text, font=font, fill=tuple(int(c) for c in color))
return cv2.cvtColor(np.array(pil), cv2.COLOR_RGB2BGR)
def detect_color_contours(frame, lower, upper, min_area=0):
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv, lower, upper)
mask = cv2.medianBlur(mask, 5)
kernel = np.ones((5,5), np.uint8)
mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)
contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
return [c for c in contours if cv2.contourArea(c) >= min_area], mask
def detect_blue_cone(frame):
contours, mask = detect_color_contours(frame, OBSTACLE_COLOR_LOWER, OBSTACLE_COLOR_UPPER, OBSTACLE_AREA_MIN)
valid = []
for c in contours:
area = cv2.contourArea(c)
if area < OBSTACLE_AREA_MIN or area > OBSTACLE_AREA_MAX:
continue
x,y,w,h = cv2.boundingRect(c)
ar = w / float(h) if h>0 else 0
if not (ASPECT_RATIO_RANGE[0] <= ar <= ASPECT_RATIO_RANGE[1]):
continue
extent = area / float(w*h) if (w*h)>0 else 0
if extent >= EXTENT_THRESHOLD:
continue
valid.append((area,(x,y,w,h)))
if not valid:
return False, None, mask
valid.sort(key=lambda x: x[0], reverse=True)
return True, valid[0][1], mask
def detect_crosswalk(frame):
# 仅检测下半 ROI，避免误触发
roi = frame[int(H*ROI_RATIO):, :]
gray = cv2.cvtColor(roi, cv2.COLOR_BGR2GRAY)
_, binary = cv2.threshold(gray, 200, 255, cv2.THRESH_BINARY)
ratio = cv2.countNonZero(binary) / float(roi.shape[0] * roi.shape[1])
return ratio >= ZEBRA_RATIO_THRESH
def generate_black_text_mask(frame):
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
mask_blue = cv2.inRange(hsv, SIGN_COLOR_LOWER, SIGN_COLOR_UPPER)
kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (3,3))
mask_blue = cv2.morphologyEx(mask_blue, cv2.MORPH_OPEN, kernel)
mask_blue = cv2.morphologyEx(mask_blue, cv2.MORPH_CLOSE, kernel)
contours, _ = cv2.findContours(mask_blue, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
mask_sym = None
if contours:
for cnt in contours:
if cv2.contourArea(cnt) >= SIGN_AREA_MIN:
x,y,w,h = cv2.boundingRect(cnt)
pad = 3
x0,y0 = max(0,x-pad), max(0,y-pad)
x1,y1 = min(frame.shape[1], x+w+pad), min(frame.shape[0], y+h+pad)
roi = frame[y0:y1, x0:x1]
gray = cv2.cvtColor(roi, cv2.COLOR_BGR2GRAY)
gray = cv2.equalizeHist(gray)
_, mask_sym = cv2.threshold(gray, ARROW_BINARY_THRESH, 255, cv2.THRESH_BINARY)
mask_sym = cv2.morphologyEx(mask_sym, cv2.MORPH_OPEN, np.ones((2,2),np.uint8))
break
original = mask_sym if mask_sym is not None else mask_blue
return cv2.bitwise_not(original)
def model_infer(mask, session, input_name, model_input_size, class_names):
mask_resized = cv2.resize(mask, model_input_size)
if len(mask_resized.shape) == 2:
mask_resized = cv2.cvtColor(mask_resized, cv2.COLOR_GRAY2BGR)
mask_input = mask_resized[:,:,::-1].transpose(2,0,1).astype(np.float32)/255.0
mask_input = np.expand_dims(mask_input, axis=0)
outputs = session.run(None, {input_name: mask_input})[0]
max_conf = -1.0
best_cls = "Waiting..."
for det in outputs:
conf = float(det[4])
cls = int(det[5])
if conf > max_conf and conf >= CONF_THRESHOLD and cls < len(class_names):
max_conf = conf
best_cls = class_names[cls]
return best_cls, max_conf
def get_lane_error(frame):
height, width = frame.shape[:2]
roi_y_start = int(height * ROI_RATIO)
roi = frame[roi_y_start:height, :]
roi_h = roi.shape[0]
if roi.size == 0:
return None, (None,None,None,[], None)
gray_roi = cv2.cvtColor(roi, cv2.COLOR_BGR2GRAY)
clahe = cv2.createCLAHE(clipLimit=3.0, tileGridSize=(4,4))
enhanced = clahe.apply(gray_roi)
blur = cv2.GaussianBlur(enhanced, (3,3), 0)
_, binary = cv2.threshold(blur, WHITE_THRESHOLD, 255, cv2.THRESH_BINARY)
kernel = np.ones((3,3),np.uint8)
binary = cv2.morphologyEx(binary, cv2.MORPH_CLOSE, kernel, iterations=2)
lines = cv2.HoughLinesP(binary, 1, np.pi/180, 20, minLineLength=MIN_LINE_LENGTH, maxLineGap=MAX_LINE_GAP)
left_lines, right_lines = [], []
if lines is not None:
for line in lines:
x1,y1,x2,y2 = line[0]
dx = x2-x1
dy = y2-y1
if dx == 0:
continue
angle = abs(np.arctan(dy/dx)*180/np.pi)
if not (ANGLE_RANGE[0] < angle < ANGLE_RANGE[1]):
continue
if y1 < y2:
x1,y1,x2,y2 = x2,y2,x1,y1
if x1 < width/2:
left_lines.append([x1,y1,x2,y2])
else:
right_lines.append([x1,y1,x2,y2])
def fit_line(lines_list):
if len(lines_list) < 2:
return None
points=[]
for x1,y1,x2,y2 in lines_list:
points.append((x1,y1,2))
points.append((x2,y2,1))
xs=np.array([p[0] for p in points])
ys=np.array([p[1] for p in points])
weights=np.array([p[2] for p in points])
try:
coeff = np.polyfit(ys, xs, 1, w=weights)
y_min, y_max = 0, roi_h-1
x_min = int(coeff[0]*y_min + coeff[1])
x_max = int(coeff[0]*y_max + coeff[1])
return [x_min, y_min, x_max, y_max]
except:
return None
left_fit = fit_line(left_lines)
right_fit = fit_line(right_lines)
mid_points = []
error = None
if left_fit and right_fit:
for y_roi in range(0, roi_h, 10):
left_coeff = np.polyfit([left_fit[1], left_fit[3]],[left_fit[0], left_fit[2]],1)
right_coeff = np.polyfit([right_fit[1], right_fit[3]],[right_fit[0], right_fit[2]],1)
left_x = left_coeff[0]*y_roi + left_coeff[1]
right_x = right_coeff[0]*y_roi + right_coeff[1]
mid_x = int((left_x + right_x)/2)
mid_y = y_roi + roi_y_start
mid_points.append((mid_x, mid_y))
image_center_x = width//2
current_mid_x = mid_points[-1][0] if mid_points else image_center_x
error = current_mid_x - image_center_x
elif left_fit and not right_fit:
for y_roi in range(0, roi_h, 10):
left_coeff = np.polyfit([left_fit[1], left_fit[3]],[left_fit[0], left_fit[2]],1)
left_x = left_coeff[0]*y_roi + left_coeff[1]
target_x = left_x + SINGLE_LINE_OFFSET
mid_points.append((int(target_x), y_roi + roi_y_start))
image_center_x = width//2
current_target_x = mid_points[-1][0] if mid_points else image_center_x
error = current_target_x - image_center_x
elif right_fit and not left_fit:
for y_roi in range(0, roi_h, 10):
right_coeff = np.polyfit([right_fit[1], right_fit[3]],[right_fit[0], right_fit[2]],1)
right_x = right_coeff[0]*y_roi + right_coeff[1]
target_x = right_x - SINGLE_LINE_OFFSET
mid_points.append((int(target_x), y_roi + roi_y_start))
image_center_x = width//2
current_target_x = mid_points[-1][0] if mid_points else image_center_x
error = current_target_x - image_center_x
return error, (left_fit, right_fit, roi_y_start, mid_points, binary)
# -----------------------------
# 主控制逻辑
# -----------------------------
def main():
# 初始化摄像头
cap = cv2.VideoCapture(CAM_INDEX)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, W)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, H)
cap.set(cv2.CAP_PROP_FPS, FPS_TARGET)
time.sleep(1.0)
if not cap.isOpened():
print("[ERR] 摄像头打开失败")
return
# 初始化硬件
try:
hw = HardwareController()
except Exception as e:
print("[ERR] 硬件初始化失败:", e)
cap.release()
return
# 加载模型（箭头 + AB）
try:
arrow_sess = onnxruntime.InferenceSession(ARROW_MODEL_PATH, providers=["CPUExecutionProvider"])
arrow_input = arrow_sess.get_inputs()[0].name
arrow_size = (arrow_sess.get_inputs()[0].shape[3], arrow_sess.get_inputs()[0].shape[2])
print("[MODEL] Arrow loaded:", arrow_size)
except Exception as e:
print("[ERR] Arrow model load failed:", e)
hw.cleanup(); cap.release(); return
try:
ab_sess = onnxruntime.InferenceSession(AB_MODEL_PATH, providers=["CPUExecutionProvider"])
ab_input = ab_sess.get_inputs()[0].name
ab_size = (ab_sess.get_inputs()[0].shape[3], ab_sess.get_inputs()[0].shape[2])
print("[MODEL] AB loaded:", ab_size)
except Exception as e:
print("[ERR] AB model load failed:", e)
hw.cleanup(); cap.release(); return
# 状态 & 标志
state = STATE_WAIT_START
start_time = None
last_save_time = time.time()
crosswalk_start = None
lane_change_dir = None
lane_change_confirm_count = 0
lane_change_done = False
ab_result = None
pay_start = None
pay_done = False
# 保证任务只执行一次的标志（除了 TRACK）
done_flags = {
STATE_START: False,
STATE_CROSSWALK: False,
STATE_LANE_CHANGE: False,
STATE_AB_RECOG: False,
STATE_PARK: False,
STATE_PAY: False
}
print("[SYSTEM] 全自动系统就绪，自动检测拆挡板（蓝色标识）后发车")
# 用于判断挡板是否存在：使用 SIGN_COLOR_LOWER/UPPER 掩码
def barrier_present(frame):
cnts, _ = detect_color_contours(frame, SIGN_COLOR_LOWER, SIGN_COLOR_UPPER, SIGN_AREA_MIN)
return len(cnts) > 0
# 自动付费（模拟）函数
def auto_pay_simulator(start_ts):
return (time.time() - start_ts) >= PAY_AUTO_SUCCESS_AFTER
# 主循环
try:
while True:
t0 = time.time()
ret, frame = cap.read()
if not ret:
print("[ERR] 读帧失败")
break
display = frame.copy()
# 始终进行循迹误差计算并调用 PID（自开始后贯穿）
lane_error, lane_info = get_lane_error(frame)
left_fit, right_fit, roi_y_start, mid_points, binary = (None, None, None, [], None)
if lane_info is not None:
left_fit, right_fit, roi_y_start, mid_points, binary = lane_info
# 状态机
if state == STATE_WAIT_START:
if barrier_present(frame):
display = draw_chinese(display, "检测到蓝色挡板，等待移除...", (10, H//2 - 20), (0,0,255))
hw.stop()
else:
if start_time is None:
start_time = time.time()
print("[STATE] 挡板移除，开始发车计时")
elif time.time() - start_time >= 0.5:
state = STATE_START
done_flags[STATE_START] = False
start_time = time.time()
print("[STATE] 切换 -> START")
display = draw_chinese(display, "等待拆挡板...", (10,30), (255,0,0))
elif state == STATE_START:
if lane_error is not None:
hw.pid_control(lane_error)
if left_fit and right_fit:
if not done_flags[STATE_START]:
print("[START] 前轮过线检测到，进入巡线避障")
done_flags[STATE_START] = True
last_save_time = time.time()
state = STATE_TRACK
print("[STATE] 切换 -> TRACK")
else:
display = draw_chinese(display, "发车等待：查找起跑线...", (10,60), (0,255,255))
elif state == STATE_TRACK:
if time.time() - last_save_time >= SAVE_INTERVAL:
ts = datetime.datetime.now().strftime("%Y%m%d_%H%M%S")
path = os.path.join(DATA_SAVE_DIR, f"track_{ts}.jpg")
cv2.imwrite(path, frame)
print("[DATA] 已保存:", path)
last_save_time = time.time()
if lane_error is not None:
hw.pid_control(lane_error)
else:
hw.set_servo_angle(SERVO_MID_ANGLE)
hw.set_motor_speed(FORWARD_LOW)
cone_found, bbox, mask = detect_blue_cone(frame)
if cone_found and bbox is not None:
x,y,w,h = bbox
center_x = x + w//2
bottom_y = y + h
if bottom_y > int(H*0.5):
display = draw_chinese(display, "近距离障碍，微调避障", (10,90), (0,255,255))
if center_x < W//2:
hw.set_servo_angle(min(SERVO_MAX_ANGLE, hw.current_angle + 8))
else:
hw.set_servo_angle(max(SERVO_MIN_ANGLE, hw.current_angle - 8))
hw.set_motor_speed(FORWARD_LOW)
if detect_crosswalk(frame) and not done_flags[STATE_CROSSWALK]:
print("[TRACK] 斑马线检测到，切换 -> CROSSWALK")
state = STATE_CROSSWALK
crosswalk_start = time.time()
done_flags[STATE_CROSSWALK] = True
hw.stop()
display = draw_chinese(display, "阶段：巡线避障", (10,30), (0,255,0))
elif state == STATE_CROSSWALK:
elapsed = time.time() - crosswalk_start
display = draw_chinese(display, f"斑马线停车倒计时: {max(0, int(CROSSWALK_STOP_DURATION - elapsed))}s", (10,60), (0,255,255))
display = draw_chinese(display, CROSSWALK_SPEECH, (10, 100), (255,255,0))
hw.stop()
if elapsed >= CROSSWALK_STOP_DURATION:
print("[CROSSWALK] 停车完成，切换 -> LANE_CHANGE")
state = STATE_LANE_CHANGE
lane_change_dir = None
lane_change_confirm_count = 0
lane_change_done = False
done_flags[STATE_LANE_CHANGE] = False
elif state == STATE_LANE_CHANGE:
display = draw_chinese(display, "阶段：变道任务（识别箭头）", (10,30), (255,165,0))
if lane_error is not None:
hw.pid_control(lane_error)
# 识别箭头并连续确认
mask = generate_black_text_mask(frame)
pred, conf = model_infer(mask, arrow_sess, arrow_input, arrow_size, ARROW_CLASS_NAMES)
if pred != "Waiting...":
# 连续确认逻辑：如果与上次相同则计数，否则重置为 1
if lane_change_dir is None:
lane_change_dir = pred
lane_change_confirm_count = 1
else:
if pred == lane_change_dir:
lane_change_confirm_count += 1
else:
lane_change_dir = pred
lane_change_confirm_count = 1
display = draw_chinese(display, f"箭头识别: {pred} ({conf:.2f}) 连续: {lane_change_confirm_count}/{REQUIRED_CONSECUTIVE}", (10,70), (255,255,0))
if lane_change_confirm_count >= REQUIRED_CONSECUTIVE:
# 检查目标侧近障碍，若有则先避障再变道
target_is_left = (lane_change_dir == "arrow_left")
cone_found2, bbox2, _ = detect_blue_cone(frame)
need_delay = False
if cone_found2 and bbox2:
bx,by,bw,bh = bbox2
bcenter_x = bx + bw//2
by_bottom = by+bh
if target_is_left and bcenter_x < W//2 and by_bottom > int(H*0.4):
need_delay = True
if (not target_is_left) and bcenter_x > W//2 and by_bottom > int(H*0.4):
need_delay = True
if need_delay:
display = draw_chinese(display, "变道前方有近障碍，延迟并避障", (10,110), (0,255,255))
if target_is_left:
hw.set_servo_angle(min(SERVO_MAX_ANGLE, hw.current_angle + 12))
else:
hw.set_servo_angle(max(SERVO_MIN_ANGLE, hw.current_angle - 12))
hw.set_motor_speed(FORWARD_LOW)
time.sleep(0.6)
lane_change_dir = None
lane_change_confirm_count = 0
continue
# 执行变道微调（不切断 PID）
print(f"[LANE_CHANGE] 执行变道：{lane_change_dir}")
if lane_change_dir == "arrow_left":
hw.set_servo_angle(max(SERVO_MIN_ANGLE, hw.current_angle - 18))
else:
hw.set_servo_angle(min(SERVO_MAX_ANGLE, hw.current_angle + 18))
hw.set_motor_speed(FORWARD_BASE - 200)
time.sleep(0.6)
else:
display = draw_chinese(display, "未检测到箭头，继续巡迹搜索...", (10,70), (255,255,255))
# 检测引导区蓝黄一排以判定变道完成
blue_cnts, _ = detect_color_contours(frame, OBSTACLE_COLOR_LOWER, OBSTACLE_COLOR_UPPER, OBSTACLE_AREA_MIN)
yellow_cnts, _ = detect_color_contours(frame, YELLOW_COLOR_LOWER, YELLOW_COLOR_UPPER, OBSTACLE_AREA_MIN)
if len(blue_cnts) > 0 and len(yellow_cnts) > 0:
lane_change_done = True
hw.set_servo_angle(SERVO_MID_ANGLE)
print("[LANE_CHANGE] 引导区检测到，变道完成，切换 -> AB_RECOG")
state = STATE_AB_RECOG
done_flags[STATE_LANE_CHANGE] = True
lane_change_dir = None
lane_change_confirm_count = 0
else:
if lane_error is not None:
hw.pid_control(lane_error)
elif state == STATE_AB_RECOG:
display = draw_chinese(display, "阶段：A/B 识别", (10,30), (255,0,255))
if lane_error is not None:
hw.pid_control(lane_error)
mask = generate_black_text_mask(frame)
pred, conf = model_infer(mask, ab_sess, ab_input, ab_size, AB_CLASS_NAMES)
display = draw_chinese(display, f"A/B 识别: {pred} ({conf:.2f})", (10,70), (255,255,0))
if pred in AB_CLASS_NAMES and conf >= CONF_THRESHOLD:
ab_result = pred
print(f"[AB_RECOG] 识别到: {ab_result}，切换 -> PARK")
state = STATE_PARK
done_flags[STATE_AB_RECOG] = True
time.sleep(0.2)
elif state == STATE_PARK:
display = draw_chinese(display, "阶段：停车任务（寻找黄线）", (10,30), (0,165,255))
if lane_error is not None:
hw.pid_control(lane_error)
yellow_cnts, ymask = detect_color_contours(frame, YELLOW_COLOR_LOWER, YELLOW_COLOR_UPPER, 50)
found = False
if yellow_cnts:
cnt = max(yellow_cnts, key=cv2.contourArea)
x,y,w,h = cv2.boundingRect(cnt)
cx = x + w//2
park_area = PARK_AREA_LEFT if ab_result == "A" else PARK_AREA_RIGHT
if park_area[0] < cx < park_area[1]:
hw.stop()
print(f"[PARK] 进入 {ab_result} 区停车完成")
state = STATE_PAY
pay_start = time.time()
done_flags[STATE_PARK] = True
found = True
else:
target_cx = (park_area[0] + park_area[1]) // 2
e = target_cx - cx
hw.pid_control(e)
display = draw_chinese(display, f"调整进入{ab_result}区...", (10,70), (255,255,0))
if not found:
display = draw_chinese(display, "搜索黄线停车线...", (10,70), (255,0,0))
elif state == STATE_PAY:
display = draw_chinese(display, "阶段：扫码付费（自动模拟）", (10,30), (0,200,200))
display = draw_chinese(display, PAY_PROMPT, (10,70), (255,255,0))
elapsed = time.time() - pay_start if pay_start else 0
display = draw_chinese(display, f"付费倒计时: {max(0, int(PAY_DURATION - elapsed))}s", (10,110), (255,255,0))
if auto_pay_simulator(pay_start):
pay_done = True
print("[PAY] 模拟付费成功")
state = STATE_FINISH
done_flags[STATE_PAY] = True
elif elapsed >= PAY_DURATION:
print("[PAY] 付费超时（自动结束）")
state = STATE_FINISH
elif state == STATE_FINISH:
display = draw_chinese(display, "任务完成！", (W//2 - 80, H//2 - 10), (0,255,0))
if pay_done:
display = draw_chinese(display, f"付费成功，加分: {PAY_SCORE}", (W//2 - 120, H//2 + 30), (0,255,0))
else:
display = draw_chinese(display, "未成功付费或超时", (W//2 - 120, H//2 + 30), (0,0,255))
cv2.imshow("SmartCar Final Auto", display)
cv2.waitKey(2000)
break
# 绘制 debug 可视化（循迹线 / mask）
if mid_points:
for i in range(len(mid_points)-1):
cv2.line(display, mid_points[i], mid_points[i+1], (0,0,255), 3)
if binary is not None:
try:
thumb = cv2.resize(binary, (160, 120))
display[10:130, W-170:W-10] = cv2.cvtColor(thumb, cv2.COLOR_GRAY2BGR)
except:
pass
display = draw_chinese(display, f"状态: {state}", (8, 8), (255,255,255))
display = draw_chinese(display, f"舵机: {int(hw.current_angle)}", (8, 36), (255,200,0))
cv2.imshow("SmartCar Final Auto", display)
if cv2.waitKey(1) & 0xFF == ord('q'):
print("[USER] 手动退出")
break
# 按 FPS 控制循环频率
elapsed_loop = time.time() - t0
time.sleep(max(0, 1.0 / FPS_TARGET - elapsed_loop))
except KeyboardInterrupt:
print("[USER] KeyboardInterrupt")
finally:
cap.release()
cv2.destroyAllWindows()
if 'hw' in locals():
hw.cleanup()
print("[SYSTEM] 退出，清理完成")
if __name__ == "__main__":
os.environ.pop("QT_QPA_PLATFORM_PLUGIN_PATH", None)
main()
进一步优化代码，先是对已用的库添加注释，再把代码的控制速度角度的代码优化并且添加注释，方便根据循迹、避障等调整代码参数
订阅专栏 解锁全文
扫一扫
922
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
