# -*- coding: utf-8 -*-
import numpy as np
import cv2
import time
import math
from pyzbar import pyzbar
from connect_uav import UPUavControl
from multiprocessing import Process
from multiprocessing.managers import BaseManager
cap = cv2.VideoCapture(0)
no_slice = 4
center = (240, 227)
# 全局速度限制
MAX_SPEED = 250
MAX_TURN_SPEED = 180
MAX_ADJUST_SPEED = 100
class LineFollower():
def __init__(self):
self.is_follow = True
self.no_line_count = 0
self.last_angle = 0
self.turn_count = 0
# 添加当前速度状态
self.current_speed = {'forward': 0, 'turn': 0, 'lateral': 0}
# 二维码相关属性
self.scan_content = ""
self.adjusting_for_landing = False
self.qr_detected = False
self.qr_adjust_count = 0
self.no_qr_count = 0
self.last_qr_position = None # 记录上一次二维码位置
self.last_qr_size = 0 # 记录上一次二维码大小
# 简化控制参数
self.min_qr_size = 100 # 二维码最小尺寸(像素)用于降落判断
BaseManager.register('UPUavControl', UPUavControl)
manager = BaseManager()
manager.start()
self.airplanceApi = manager.UPUavControl()
controllerAir = Process(name="controller_air", target=self.api_init)
controllerAir.start()
time.sleep(1)
self.airplanceApi.onekey_takeoff(60)
time.sleep(5)
# 起飞后向前飞行3秒 - 提高速度
print("起飞完成,开始向前飞行...")
self.set_speed('forward', 200) # 提高初始飞行速度
time.sleep(3) # 飞行3秒
# 停止向前移动
self.set_speed('forward', 0)
print("向前飞行完成,开始巡线任务")
self.start_video()
super(LineFollower, self).__init__()
def set_speed(self, direction, speed):
"""平滑速度过渡并限制最大速度"""
# 限制最大速度
speed = min(speed, MAX_SPEED)
# 平滑过渡 - 限制加速度
current = self.current_speed[direction]
if abs(speed - current) > 40:
speed = current + 40 if speed > current else current - 40
# 更新API
if direction == 'forward':
self.airplanceApi.move_forward(speed)
elif direction == 'turn':
if speed > 0:
self.airplanceApi.turn_left(min(speed, MAX_TURN_SPEED))
else:
self.airplanceApi.turn_right(min(-speed, MAX_TURN_SPEED))
elif direction == 'lateral':
if speed > 0:
self.airplanceApi.move_right(min(speed, MAX_ADJUST_SPEED))
else:
self.airplanceApi.move_left(min(-speed, MAX_ADJUST_SPEED))
# 更新状态
self.current_speed[direction] = speed
return speed
def api_init(self):
print("飞行控制进程启动")
time.sleep(0.5)
self.airplanceApi.setServoPosition(90)
def start_video(self):
last_frame_time = time.time()
while cap.isOpened():
start_time = time.time()
ret, frame = cap.read()
if not ret:
break
# 使用ROI减少处理区域 (保留中间区域)
height, width = frame.shape[:2]
roi_top = height // 4
roi_bottom = height * 3 // 4
roi_left = width // 4
roi_right = width * 3 // 4
roi_frame = frame[roi_top:roi_bottom, roi_left:roi_right]
# 缩放处理
frame = cv2.resize(roi_frame, (0, 0), fx=0.75, fy=0.75)
# 根据状态调整二维码检测频率
qr_check_interval = 1 if self.adjusting_for_landing else 3
if self.qr_adjust_count % qr_check_interval == 0:
self.decode(frame)
self.qr_adjust_count += 1
# 如果识别到降落二维码且满足条件,执行降落
if self.scan_content == "landed" and self.adjusting_for_landing:
# 检查二维码是否在中心且足够大
if self.is_qr_centered_and_large():
print("二维码在中心且足够大,执行降落")
self.airplanceApi.land()
time.sleep(3)
cap.release()
cv2.destroyAllWindows()
return # 结束程序
# 处理二维码丢失情况
if not self.qr_detected and self.adjusting_for_landing:
self.no_qr_count += 1
if self.no_qr_count > 30: # 连续30帧未检测到二维码
print("二维码丢失,停止调整")
self.adjusting_for_landing = False
self.no_qr_count = 0
else:
self.no_qr_count = 0
# 如果正在调整位置准备降落,跳过黑线识别
if self.adjusting_for_landing:
# 显示调整状态
display_frame = frame.copy()
status_text = "Adjusting for Landing"
cv2.putText(display_frame, status_text, (50, 50),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
cv2.imshow('line_tracking', display_frame)
# 动态帧率处理
processing_time = time.time() - start_time
delay = max(1, int(30 - processing_time * 1000)) # 保持约30FPS
if cv2.waitKey(delay) & 0xFF == ord('q'):
break
continue
# 正常巡线处理
img = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
img = self.remove_background(img, True)
slices, cont_cent = self.slice_out(img, no_slice)
img = self.repack(slices)
if all(c[0] == 0 and c[1] == 0 for c in cont_cent):
self.no_line_count += 1
print(f"无法识别线条,计数: {self.no_line_count}")
if self.no_line_count >= 2: # 更快响应
print("连续多帧识别不到黑线,尝试寻找")
search_speed = 150 # 提高搜索速度
if self.no_line_count % 4 == 0: # 优化搜索模式
self.set_speed('turn', search_speed)
print("向左寻找")
elif self.no_line_count % 4 == 2:
self.set_speed('turn', -search_speed)
print("向右寻找")
else:
self.no_line_count = 0
self.line(img, center, cont_cent)
# 显示图像
cv2.imshow('line_tracking', img)
# 动态帧率处理
processing_time = time.time() - start_time
delay = max(1, int(30 - processing_time * 1000)) # 保持约30FPS
if cv2.waitKey(delay) & 0xFF == ord('q'):
break
cap.release()
cv2.destroyAllWindows()
def is_qr_centered_and_large(self):
"""检查二维码是否在中心区域且足够大"""
if not self.last_qr_position:
return False
# 获取图像尺寸
height, width = 360, 480 # 假设图像尺寸为480x360 (0.75缩放后)
# 中心区域定义 (中心±100像素)
center_region_x = (width / 2 - 100, width / 2 + 100)
center_region_y = (height / 2 - 100, height / 2 + 100)
# 检查二维码中心位置
center_x, center_y = self.last_qr_position
in_center = (center_region_x[0] <= center_x <= center_region_x[1] and
center_region_y[0] <= center_y <= center_region_y[1])
# 检查二维码大小
large_enough = self.last_qr_size > self.min_qr_size
print(
f"QR Center: ({center_x:.1f}, {center_y:.1f}), Size: {self.last_qr_size}, In Center: {in_center}, Large Enough: {large_enough}")
return in_center and large_enough
def get_contour_center(self, contour):
m = cv2.moments(contour)
if m["m00"] == 0:
return [0, 0]
x = int(m["m10"] / m["m00"])
y = int(m["m01"] / m["m00"])
return [x, y]
def process(self, image):
_, thresh = cv2.threshold(image, 100, 255, cv2.THRESH_BINARY_INV)
contours, _ = cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
if contours:
main_contour = max(contours, key=cv2.contourArea)
cv2.drawContours(image, [main_contour], -1, (150, 150, 150), 2)
contour_center = self.get_contour_center(main_contour)
cv2.circle(image, tuple(contour_center), 2, (150, 150, 150), 2)
else:
return image, (0, 0)
return image, contour_center
def slice_out(self, im, num):
cont_cent = []
height, width = im.shape[:2]
sl = int(height / num)
sliced_imgs = []
for i in range(num):
part = sl * i
crop_img = im[part:part + sl, 0:width]
processed = self.process(crop_img)
sliced_imgs.append(processed[0])
cont_cent.append(processed[1])
return sliced_imgs, cont_cent
def remove_background(self, image, b):
if b:
lower = np.array([0], dtype="uint8")
upper = np.array([80], dtype="uint8")
mask = cv2.inRange(image, lower, upper)
image = cv2.bitwise_and(image, image, mask=mask)
image = cv2.bitwise_not(image, image, mask=mask)
image = (255 - image)
return image
def repack(self, images):
im = images[0]
for i in range(1, len(images)):
im = np.concatenate((im, images[i]), axis=0)
return im
def calculate_path_angle(self, cont_cent):
"""计算路径弯曲角度"""
valid_points = [c for c in cont_cent if c != (0, 0)]
if len(valid_points) < 2:
return self.last_angle
top_point = valid_points[0]
bottom_point = valid_points[-1]
dx = bottom_point[0] - top_point[0]
dy = (len(valid_points) - 1) * 65
if dy == 0:
return self.last_angle
angle_rad = math.atan2(dx, dy)
angle_deg = math.degrees(angle_rad)
self.last_angle = angle_deg
return angle_deg
def line(self, image, center, cont_cent):
cv2.line(image, (0, 65), (480, 65), (30, 30, 30), 1)
cv2.line(image, (0, 130), (480, 130), (30, 30, 30), 1)
cv2.line(image, (0, 195), (480, 195), (30, 30, 30), 1)
cv2.line(image, (240, 0), (240, 260), (30, 30, 30), 1)
prev_point = None
for i, point in enumerate(cont_cent):
if point != (0, 0):
y_pos = point[1] + 65 * i
cv2.circle(image, (point[0], y_pos), 3, (0, 0, 255), -1)
if prev_point is not None:
cv2.line(image, prev_point, (point[0], y_pos), (0, 255, 0), 2)
prev_point = (point[0], y_pos)
path_angle = self.calculate_path_angle(cont_cent)
weights = [0.1, 0.2, 0.3, 0.4]
valid = [(c[0], w) for c, w in zip(cont_cent, weights) if c != (0, 0)]
if not valid:
print("未检测到有效中心点")
return
offset_center = sum((x - 240) * w for x, w in valid)
print(f"cont_cent: {cont_cent}")
print(f"offset_center: {offset_center:.2f}")
print(f"Path angle: {path_angle:.2f} degrees")
if self.is_follow:
# 角度控制优化
if abs(path_angle) > 15:
self.turn_count += 1
if self.turn_count > 2: # 减少等待帧数
# 动态速度 - 角度越大转得越快
turn_speed = min(MAX_TURN_SPEED, 100 + abs(int(path_angle)))
actual_speed = self.set_speed('turn', turn_speed if path_angle > 0 else -turn_speed)
print(f"转向 {'左' if path_angle > 0 else '右'}, 速度: {abs(actual_speed)}")
else:
self.set_speed('forward', 150)
print("准备转向中...")
else:
self.turn_count = 0
# 偏移控制优化 - 动态阈值
offset_threshold = max(60, 120 - abs(path_angle) * 2)
if abs(offset_center) > offset_threshold:
# 动态速度 - 偏移越大移动越快
move_speed = min(MAX_ADJUST_SPEED, 120 + int(abs(offset_center) / 2))
actual_speed = self.set_speed('lateral', move_speed if offset_center > 0 else -move_speed)
print(f"{'右移' if offset_center > 0 else '左移'}, 速度: {abs(actual_speed)}")
else:
# 提高直行速度
actual_speed = self.set_speed('forward', 220)
print(f"前进, 速度: {actual_speed}")
# 优化二维码识别方法
def decode(self, image):
"""识别图像中的二维码"""
# 重置本次检测状态
self.qr_detected = False
# 图像预处理 - 提高识别率
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
# 增强对比度
gray = cv2.convertScaleAbs(gray, alpha=1.5, beta=20)
# 中值滤波去噪
gray = cv2.medianBlur(gray, 3)
# 尝试识别二维码
barcodes = pyzbar.decode(gray)
for barcode in barcodes:
(x, y, w, h) = barcode.rect
# 过滤太小的二维码(可能是误识别)
if w < 30 or h < 30:
continue
cv2.rectangle(image, (x, y), (x + w, y + h), (255, 0, 0), 2)
try:
barcodeData = barcode.data.decode("utf-8")
except:
continue
text = "{}".format(barcodeData)
cv2.putText(image, text, (x, y - 10), cv2.FONT_HERSHEY_SIMPLEX,
0.8, (255, 0, 0), 2)
center_x = x + w / 2
center_y = y + h / 2
cv2.circle(image, (int(center_x), int(center_y)), 5, (0, 255, 0), -1)
self.qr_detected = True
self.last_qr_position = (center_x, center_y)
self.last_qr_size = max(w, h) # 记录二维码大小
# 只处理内容为"landed"的二维码
if barcodeData == "landed":
self.scan_content = "landed"
if not self.adjusting_for_landing:
print(f"识别到降落二维码: {text}")
self.adjusting_for_landing = True
# 简化调整逻辑:只关注二维码位置和大小
# 获取图像尺寸
height, width = image.shape[:2]
# 中心区域定义 (中心±100像素)
center_region_x = (width / 2 - 100, width / 2 + 100)
center_region_y = (height / 2 - 100, height / 2 + 100)
# 检查二维码中心位置
in_center = (center_region_x[0] <= center_x <= center_region_x[1] and
center_region_y[0] <= center_y <= center_region_y[1])
# 调整无人机位置
if not in_center:
# 水平调整 - 提高速度并添加比例控制
h_offset = abs(center_x - width/2)
h_speed = min(MAX_ADJUST_SPEED, 40 + int(h_offset / 4))
if center_x < width / 2:
self.airplanceApi.move_left(h_speed)
print(f"向左移动, 速度: {h_speed}")
else:
self.airplanceApi.move_right(h_speed)
print(f"向右移动, 速度: {h_speed}")
# 垂直调整 - 提高速度
v_speed = 40
if center_y < height / 2:
self.airplanceApi.move_up(v_speed)
print(f"向上移动, 速度: {v_speed}")
else:
self.airplanceApi.move_down(v_speed)
print(f"向下移动, 速度: {v_speed}")
else:
# 当在中心区域时,停止水平移动
self.airplanceApi.move_left(0)
self.airplanceApi.move_right(0)
# 如果二维码不够大,下降
if self.last_qr_size < self.min_qr_size:
self.airplanceApi.move_down(40)
print(f"向下移动靠近二维码, 速度: 40")
else:
self.airplanceApi.move_down(0)
print("二维码大小合适")
# 强制降落条件:当二维码在中心且足够大时
if in_center and self.last_qr_size > self.min_qr_size:
print("二维码在中心且足够大,准备降落")
break # 一次只处理一个二维码
if __name__ == '__main__':
line_follower = LineFollower()
帮我看看这个代码怎么样