import time, math
from maix import nn, camera, display, image
import serial
class YOLOv2:
def __init__(self, model_path, labels, anchors, net_in_size, net_out_size):
self.labels = labels
self.anchors = anchors
self.net_in_size = net_in_size
self.net_out_size = net_out_size
print("-- load model:", model_path)
self.model = nn.load(model_path)
print("-- load ok")
print("-- init yolo2 decoder")
self._decoder = nn.decoder.Yolo2(len(labels), anchors, net_in_size=net_in_size, net_out_size=net_out_size)
print("-- init complete")
def run(self, img, nms=0.3, threshold=0.5):
out = self.model.forward(img, layout="hwc")
boxes, probs = self._decoder.run(out, nms=nms, threshold=threshold, img_size=self.net_in_size)
return boxes, probs
def draw(self, img, boxes, probs):
for i, box in enumerate(boxes):
class_id = probs[i][0]
prob = probs[i][1][class_id]
msg = "{}:{:.2f}%".format(self.labels[class_id], prob * 100)
img.draw_rectangle(box[0], box[1], box[0] + box[2], box[1] + box[3], color=(255, 255, 255), thickness=2)
img.draw_string(box[0] + 2, box[1] + 2, msg, scale=1.2, color=(255, 255, 255), thickness=2)
class RobotControl(object):
def __init__(self):
self.img_code_length = 560 # 640
self.img_barr_length = 120
# self.red_threshold = [(2, 95, -51, 20, -33, 24)]
self.red_threshold = [(0, 95, -90, 20, -66, 24)]
self.img = None # 存图片
camera.config(size=(224, 224))
# 通信
self.ser = serial.Serial("/dev/ttyS1", 19200, timeout=1)
# YOLO模型基本参数
self.input_size = (224, 224)
# self.model = "model-88559.awnn.mud"
# self.model = "/mnt/UDISK/app/model-88559.awnn.mud" # 要改
self.model = "model-88559.awnn.mud" if __name__ == '__main__' else "/mnt/UDISK/app/model-88559.awnn.mud" # 三目运算符取模型参数
self.labels = ['code']
self.anchors = [1.44, 0.97, 1.47, 1.16, 1.62, 1.31, 1.25, 0.94, 1.81, 1.53]
# 初始化YOLOv2模型
self.yolov2 = YOLOv2(self.model, self.labels, self.anchors, self.input_size,
(self.input_size[0] // 32, self.input_size[1] // 32))
# yolo检测出东西后的值与标志位
self.is_detect = False
# v831模式切换
self.mode = 0 # 0 YOLO检测 1 二维码检测 2 障碍物检测
self.need_change_mode = 0 # 需要变换模式的标志位
self.y_low = 100 # 障碍物靠近的距离
self.thou_pixels = 1000 # 障碍物的像素点，还未具体测量
def send_to_arduino(self, mes):
print("send_message:", mes) # 调试用
send_str = "{" + str(mes) + "&}"
self.ser.write(send_str.encode("utf-8"))
def recv_from_arduino(self):
recv = self.ser.read()
if recv:
return recv.decode("utf-8")
else:
return None
def change_mode_qrcode(self):
camera.config(size=(self.img_code_length, self.img_code_length))
print("mode_change_success! qrcode")
def change_mode_barr(self):
camera.config(size=(self.img_barr_length, self.img_barr_length))
print("mode_change_success! barr")
def change_mode_yolo(self):
camera.config(size=self.input_size)
print("mode_change_success! yolo")
def test_yolo(self):
self.change_mode_yolo()
while 1:
img = camera.capture()
boxes, probs = self.yolov2.run(img, nms=0.3, threshold=0.5)
# boxes: [[46, 156, 46, 31], [45, 124, 51, 41], [0, 56, 57, 48]] x, y, w, h
self.yolov2.draw(img, boxes, probs)
# 放在机器人上后，middle_y越小二维码越靠近机器人
if boxes:
boxes = boxes[0]
middle_y = boxes[1] + boxes[3] // 2
middle_x = boxes[0] + boxes[2] // 2 # 这个值越大二维码在越左边
print(middle_y, middle_x)
display.show(img)
def test_qrcode(self):
self.change_mode_qrcode()
while 1:
img = camera.capture()
mks = img.find_qrcodes()
for mk in mks:
# 二维码信息
string = mk['payload']
# 内框数据
x1, y1 = mk['corners'][0] # 访问字典的列表
x2, y2 = mk['corners'][1]
x3, y3 = mk['corners'][2]
x4, y4 = mk['corners'][3]
# 画内框
img.draw_line(x1, y1, x2, y2, color=(0, 255, 0), thickness=3)
img.draw_line(x2, y2, x3, y3, color=(0, 255, 0), thickness=3)
img.draw_line(x3, y3, x4, y4, color=(0, 255, 0), thickness=3)
img.draw_line(x4, y4, x1, y1, color=(0, 255, 0), thickness=3)
display.show(img)
def test_barr(self):
self.change_mode_barr()
while 1:
img = camera.capture()
blobs = img.find_blobs(self.red_threshold, merge=True, invert=True)
most_pixels = 0
max_x = 0
# print("blobs:", blobs)
"""
{'x': 0, 'y': 0, 'w': 50, 'h': 177, 'pixels': 6343,
'centroid_x': 18.807661056518555, 'centroid_y': 76.3976058959961, 'rotation': 1.645219087600708,
'code': 1, 'count': 1}
"""
if blobs:
for n in range(len(blobs)):
if blobs[n]["pixels"] > most_pixels:
most_pixels = blobs[n]["pixels"]
max_x = n
i = blobs[max_x]
img.draw_rectangle(i["x"], i["y"], i["x"] + i["w"], i["y"] + i["h"],
color=(0, 0, 255), thickness=1) # 将找到的颜色区域画出来
cx = i["x"] + i["w"] // 2
cy = i["y"] + i["h"] // 2
# pixels = i["pixels"]
rotation = i["rotation"]
rotation = math.degrees(rotation) - 90
print(cx, cy, rotation, pixels)
display.show(img)
# 此函数专门用于找二维码并返回二维码的中点值
def yolo_find(self) -> int:
boxes, probs = self.yolov2.run(self.img, nms=0.3, threshold=0.5)
self.yolov2.draw(self.img, boxes, probs)
if boxes:
boxes = boxes
# middle_y = boxes[1] + boxes[3]//2
return boxes[1] + boxes[3] // 2
else:
return 0
# 当获取到二维码位置后与mega的状态通信
def yolo_chat_arduino(self):
while 1:
recv = self.recv_from_arduino()
self.img = camera.capture()
if recv:
print("yolo获取的信息为：", recv) # 调试用
if recv == "b":
self.need_change_mode = 1
self.mode = 1
break
elif recv == "f":
self.img = camera.capture()
middle_y = self.yolo_find()
self.send_to_arduino("a" + str(middle_y))
def run(self):
while 1:
if self.mode == 0:
self.change_mode_yolo()
while 1:
self.img = camera.capture()
middle_y = self.yolo_find()
display.show(self.img)
if middle_y:
self.yolo_chat_arduino()
# 将整个流程装到一个函数，不做复用,这样写方便状态机推进，不容易乱
def just_run(self):
# 第一阶段，先找二维码
print("second one!")
self.change_mode_yolo()
# is_true = 0
while 1:
self.img = camera.capture()
boxes, probs = self.yolov2.run(self.img, nms=0.3, threshold=0.5)
self.yolov2.draw(self.img, boxes, probs)
display.show(self.img)
# recv = self.recv_from_arduino()
# if recv:
# # is_true = 1
# break
if boxes:
boxes = boxes[0]
middle_y = boxes[1] + boxes[3]//2
if middle_y <= 135:
self.send_to_arduino("a"+str(middle_y)) # 找到二维码后发送找到的标志位
break
# 特殊情况直接进行其他判断
# if is_true:
# while 1:
# self.img = camera.capture()
# display.show(self.img)
# recv = self.recv_from_arduino()
# if recv:
# print("recv:", recv)
# if recv == 's':
# time.sleep(2) # 停两秒
# self.img = camera.capture()
# mks = self.img.find_qrcodes()
# if mks:
# mk = mks[0]
# # 二维码信息
# string = mk['payload']
# self.send_to_arduino("c" + string)
# # 内框数据
# x1, y1 = mk['corners'][0] # 访问字典的列表
# x2, y2 = mk['corners'][1]
# x3, y3 = mk['corners'][2]
# x4, y4 = mk['corners'][3]
# # 画内框
# self.img.draw_line(x1, y1, x2, y2, color=(0, 255, 0), thickness=3)
# self.img.draw_line(x2, y2, x3, y3, color=(0, 255, 0), thickness=3)
# self.img.draw_line(x3, y3, x4, y4, color=(0, 255, 0), thickness=3)
# self.img.draw_line(x4, y4, x1, y1, color=(0, 255, 0), thickness=3)
# display.show(self.img)
# else:
# self.send_to_arduino("c" + "100")
# 第二阶段， 靠近二维码
print("second two!")
while 1:
self.img = camera.capture()
recv = self.recv_from_arduino()
if recv:
print("recv:", recv)
# time.sleep(3)
if recv == "f":
for i in range(15):
self.img = camera.capture()
boxes, probs = self.yolov2.run(self.img, nms=0.3, threshold=0.5)
self.yolov2.draw(self.img, boxes, probs)
display.show(self.img)
# 另写一个扫码检测的运动控制
if boxes:
boxes = boxes[0]
middle_y = boxes[1] + boxes[3] // 2
self.send_to_arduino("a" + str(middle_y)) # 找到二维码后发送找到的标志位
else:
self.send_to_arduino("a" + str(1000)) # 看不到二维码了
elif recv == "b":
break
elif recv == "n":
break
# 第三阶段，扫码
print("second three!")
self.change_mode_qrcode()
while 1:
self.img = camera.capture()
# 下面指令用来预防没扫到码的情况
recv = self.recv_from_arduino()
if recv:
if recv == "n":
break
mks = self.img.find_qrcodes()
if mks:
mk = mks[0]
# 二维码信息
string = mk['payload']
self.send_to_arduino("c" + string)
# 内框数据
x1, y1 = mk['corners'][0] # 访问字典的列表
x2, y2 = mk['corners'][1]
x3, y3 = mk['corners'][2]
x4, y4 = mk['corners'][3]
# 画内框
self.img.draw_line(x1, y1, x2, y2, color=(0, 255, 0), thickness=3)
self.img.draw_line(x2, y2, x3, y3, color=(0, 255, 0), thickness=3)
self.img.draw_line(x3, y3, x4, y4, color=(0, 255, 0), thickness=3)
self.img.draw_line(x4, y4, x1, y1, color=(0, 255, 0), thickness=3)
break
display.show(self.img)
# # 第四阶段，找障碍物
# print("second four!")
# self.change_mode_barr()
# while 1:
# self.img = camera.capture()
# blobs = self.img.find_blobs(self.red_threshold, merge=True, invert=True)
# display.show(self.img)
# most_pixels = 0
# max_x = 0
# if blobs:
# for n in range(len(blobs)):
# if blobs[n]["pixels"] > most_pixels:
# most_pixels = blobs[n]["pixels"]
# max_x = n
# i = blobs[max_x]
# # print("""i["pixels"]""", i["pixels"])
# if i["y"] <= self.y_low and i["pixels"] > self.thou_pixels:
# self.send_to_arduino("r" + "111")
# break
# # 第五阶段，靠近障碍物
# print("second five!")
# while 1:
# self.img = camera.capture()
# recv = self.recv_from_arduino()
# if recv:
# print("recv:", recv)
# if recv == "c":
# for i in range(5):
# self.img = camera.capture()
# blobs = self.img.find_blobs(self.red_threshold, merge=True, invert=True)
# most_pixels = 0
# max_x = 0
# if blobs:
# for n in range(len(blobs)):
# if blobs[n]["pixels"] > most_pixels:
# most_pixels = blobs[n]["pixels"]
# max_x = n
# i = blobs[max_x]
# self.img.draw_rectangle(i["x"], i["y"], i["x"] + i["w"], i["y"] + i["h"],
# color=(0, 0, 255), thickness=1) # 将找到的颜色区域画出来
# cx = i["x"] + i["w"] // 2 # 调整左右
# cy = i["y"] + i["h"] // 2 # 调整距离远近的
# # pixels = i["pixels"]
# rotation = math.degrees(i["rotation"]) - 90 # 朝向角度
# # 先考虑距离，再考虑角度，最后考虑左右横移
# if cy
#
# if cy >= 130:
# self.send_to_arduino(2)
# elif cy >= 105:
# self.send_to_arduino(20) # 组合运动 巡线前行+小小前进
# elif cy >= 80:
# self.send_to_arduino(4)
# else:
# if cx >= 115 and abs(rotation) >= 85:
# self.send_to_arduino(17) # 巡线前进
# elif cx <= 115:
# self.send_to_arduino(10) # 右胯一步
# elif cx >= 125:
# self.send_to_arduino(9) # 左胯一步
# elif 0 <= rotation <= 90:
# self.send_to_arduino(19) # 左转
# elif -80 <= rotation <= 0:
# self.send_to_arduino(18) # 原地右转
# else:
# self.send_to_arduino(4) # 小小前进
#
# else:
# self.send_to_arduino(11) # 越障
# display.show(self.img)
# # 障碍物判断加在这里
# """
# {'x': 0, 'y': 0, 'w': 50, 'h': 177, 'pixels': 6343,
# 'centroid_x': 18.807661056518555, 'centroid_y': 76.3976058959961, 'rotation': 1.645219087600708,
# 'code': 1, 'count': 1}
# 注意，y是越小越靠近底部 x是越小越靠近右边"""
# # define GO_STRAIGHT 2 // 直行
# # define GO_LITTLE_STRAIGHT 3 // 小直行
# # define GO_MICRO_STRAIGHT 4 // 小小直行
# # define GO_RIGHT 5 // 右转
# # define GO_LITTLE_RIGHT 6 // 小右转
# # define GO_LEFT 7 // 左转
# # define GO_LITTLE_LEFT 8 // 小左转
# # define GO_LEFT_TRANSVERSE 9 // 原地左胯一步
# # define GO_RIGHT_TRANSVERSE 10 // 原地右胯一步
# # define GO_OBSTACLE 11 // 越障
# elif recv == "b":
# break
time.sleep(3) # 休眠3秒，防止一直扫一个二维码
# 第六阶段，重复上面找二维码的阶段
ok = 0
print("second six!")
self.change_mode_yolo()
while 1:
self.img = camera.capture()
recv = self.recv_from_arduino()
if recv:
if recv == "o":
ok = 1
if ok:
self.img = camera.capture()
boxes, probs = self.yolov2.run(self.img, nms=0.3, threshold=0.5)
self.yolov2.draw(self.img, boxes, probs)
display.show(self.img)
if boxes:
boxes = boxes[0]
middle_y = boxes[1] + boxes[3]//2
if middle_y <= 135:
self.send_to_arduino("a"+str(middle_y)) # 找到二维码后发送找到的标志位
break
# 第七阶段，靠近二维码
print("second seven!")
while 1:
self.img = camera.capture()
recv = self.recv_from_arduino()
if recv:
print("recv:", recv)
# time.sleep(3)
if recv == "f":
for i in range(15): # 跳帧，防止用到了旧的图像，导致误识别
self.img = camera.capture()
boxes, probs = self.yolov2.run(self.img, nms=0.3, threshold=0.5)
self.yolov2.draw(self.img, boxes, probs)
display.show(self.img)
if boxes:
boxes = boxes[0]
middle_y = boxes[1] + boxes[3] // 2
self.send_to_arduino("a" + str(middle_y)) # 找到二维码后发送找到的标志位
else:
self.send_to_arduino("a" + str(1000)) # 看不到二维码了
elif recv == "b":
break
# 第八阶段，扫二维码
print("second eight!")
self.change_mode_qrcode()
while 1:
self.img = camera.capture()
# 下面指令用来预防没扫到码的情况
recv = self.recv_from_arduino()
if recv:
if recv == "n":
break
mks = self.img.find_qrcodes()
if mks:
mk = mks[0]
# 二维码信息
string = mk['payload']
self.send_to_arduino("c" + string)
# 内框数据
x1, y1 = mk['corners'][0] # 访问字典的列表
x2, y2 = mk['corners'][1]
x3, y3 = mk['corners'][2]
x4, y4 = mk['corners'][3]
# 画内框
self.img.draw_line(x1, y1, x2, y2, color=(0, 255, 0), thickness=3)
self.img.draw_line(x2, y2, x3, y3, color=(0, 255, 0), thickness=3)
self.img.draw_line(x3, y3, x4, y4, color=(0, 255, 0), thickness=3)
self.img.draw_line(x4, y4, x1, y1, color=(0, 255, 0), thickness=3)
break
display.show(self.img)
if __name__ == 'rpyc.core.protocol' or __name__ == '__main__':
robot = RobotControl()
# robot.test_yolo()
# robot.test_qrcode()
# robot.test_barr()
robot.just_run()
浙公网安备 33010602011771号