训练目标追踪功能模型

手动控制逻辑代码

“python/src/scenes/manual.py”为手动控制小车的核心部分。

首先导入所有必须模块和基础运动模块。

import datetime
import os
import cv2
import numpy as np
from src.actions import Advance, Stop, SetServo, TurnLeft, TurnRight, SpinClockwise, SpinAntiClockwise, BackUp, \
    ShiftLeft, ShiftRight, CustomAction
from src.actions.complex_actions import ComplexAction, TurnAround
from src.scenes.base_scene import BaseScene
from src.utils import log

基于场景的基类构建手动控制小车场景的手动类，初始化后，进入到loop循环的函数中，不断等待键盘输入的键值，再执行键值对应的指令

def loop(self):
        ret = self.init_state() # 执行初始化
        if ret:
            log.error(f'&#123;self.__class__.__name__&#125; init failed.')
            return
        frame = np.ndarray((self.height, self.width, 3), dtype=np.uint8, buffer=self.broadcaster.buf) # 拉取共享内存中的图片
        log.info(f'&#123;self.__class__.__name__&#125; loop start')
        last_action = SetServo(servo=[90, 65]) # 设置舵机角度
 
        while True:
            try:
                if not self.msg_queue.empty():
                    key = self.msg_queue.get()
                else:
                    continue
            except KeyboardInterrupt:
                self.ctrl.execute(Stop()) #捕获SIGINT之后停止小车
                break
 
            degree = 0 
            if key == 'up':
                self.speed = min(self.speed + 1, 60) #加速
            elif key == 'down':
                self.speed = max(self.speed - 1, 25) #减速
            elif key == 'left':
                last_action = ShiftLeft() #左平移
            elif key == 'right':
                last_action = ShiftRight() #右平移
            elif key == 'w':
                last_action = Advance() #前进
            elif key == 'a':
                last_action = TurnLeft() #左转
                degree = 1.1
            elif key == 's':
                last_action = BackUp() #后退
            elif key == 'd':
                last_action = TurnRight() #右转
                degree = 1.1
            elif key == 'q':
                last_action = SpinAntiClockwise() #逆时针旋转
            elif key == 'e':
                last_action = SpinClockwise() #顺时针旋转
            elif key == 'space':
                last_action = Stop() #停车
            elif key == 'esc':
                self.ctrl.execute(Stop()) #退出循环前停下小车
                break
            elif key == 'c':
                save_img = frame.copy()
                cv2.imwrite(os.path.join(self.save_dir, f'&#123;datetime.datetime.now()&#125;.jpg'), save_img) #保存当前摄像头中的画面
                log.info(f'image saved.')
            elif key == 't':
                last_action = CustomAction(motor_setting=[-62, 50, 50, -50])
            elif key == 'r':
                last_action = CustomAction(motor_setting=[55, -50, -50, 50])
            elif key == 'z':
                last_action = TurnAround() #掉头
            else:
                continue
 
            if not isinstance(last_action, ComplexAction) and not isinstance(last_action, CustomAction):
                last_action.update_speed = False
                last_action.speed_setting = last_action.generate_speed_setting(speed=self.speed, degree=degree)
                last_action.fix_speed()
            self.ctrl.execute(last_action)

父主题： 代码实现

目标检测模型代码

“python/src/models/yolov5.py”为yolov5模型的定义代码，为小车的基础运行提供核心的智能目标识别与检测功能。

示例代码定义了如何重塑图片的尺寸，并计算需要零值填充大小的功能。

def letterbox(img, new_shape=(640, 640), color=(114, 114, 114), auto=False, scaleFill=False, scaleup=True):
    # Resize image to a 32-pixel-multiple rectangle https://github.com/ultralytics/yolov3/issues/232
    shape = img.shape[:2]  # current shape [height, width]
    if isinstance(new_shape, int):
        new_shape = (new_shape, new_shape)
 
    # Scale ratio (new / old)
    r = min(new_shape[0] / shape[0], new_shape[1] / shape[1])
    if not scaleup:  # only scale down, do not scale up (for better test mAP)
        r = min(r, 1.0)
 
    # Compute padding
    ratio = r, r  # width, height ratios
    new_unpad = int(round(shape[1] * r)), int(round(shape[0] * r))
    dw, dh = new_shape[1] - new_unpad[0], new_shape[0] - new_unpad[1]  # wh padding
    if auto:  # minimum rectangle
        dw, dh = np.mod(dw, 64), np.mod(dh, 64)  # wh padding
    elif scaleFill:  # stretch
        dw, dh = 0.0, 0.0
        new_unpad = (new_shape[1], new_shape[0])
        ratio = new_shape[1] / shape[1], new_shape[0] / shape[0]  # width, height ratios
 
    dw /= 2  # divide padding into 2 sides
    dh /= 2
 
    if shape[::-1] != new_unpad:  # resize
        img = cv2.resize(img, new_unpad, interpolation=cv2.INTER_LINEAR)
    top, bottom = int(round(dh - 0.1)), int(round(dh + 0.1))
    left, right = int(round(dw - 0.1)), int(round(dw + 0.1))
    img = cv2.copyMakeBorder(img, top, bottom, left, right, cv2.BORDER_CONSTANT, value=color)  # add border
    return img, ratio, (dw, dh)

Yolov5的模型定义，以及推理的实现过程，形成最终的推理结果目标框和对应的类别名称。

class YoloV5(Model):
    def __init__(self, model_path, acl_init=True):
        super().__init__(model_path, acl_init)
        self.neth = 640
        self.netw = 640
        self.conf_threshold = 0.1
        dic = &#123;0: 'left',
               1: 'right',
               2: 'stop',
               3: 'turnaround'&#125;
        self.names = ['person', 'sports_ball', 'bicycle', 'motorcycle', 'car', 'bus', 'truck'] * 12
        self.object_list = ['person', 'sports_ball', 'bicycle', 'motorcycle', 'car', 'bus', 'truck']
        self.names = list(dic.values())
        self.object_list = list(dic.values())
 
    def infer(self, img_bgr):
        imgh, imgw = img_bgr.shape[0], img_bgr.shape[1]
        imginfo = np.array([self.neth, self.netw, imgh, imgw], dtype=np.float16)
        img_padding = letterbox(img_bgr, new_shape=(self.neth, self.netw))[0]  # padding resize bgr
 
        img = []
 
        img.append(img_padding)
        img = np.stack(img, axis=0)
        img = img[..., ::-1].transpose(0, 3, 1, 2)  # BGR tp RGB
        image_np = np.array(img, dtype=np.float32)
        image_np_expanded = image_np / 255.0
        img = np.ascontiguousarray(image_np_expanded).astype(np.float16) #将tensor的内存连续排列
        result = self.execute([img, imginfo]) #调用推理接口
        batch_boxout, boxnum = result
 
        pred_boxes = []
        idx = 0
        num_det = int(boxnum[idx][0])
        bbox = batch_boxout[idx][:num_det * 6].reshape(6, -1).transpose().astype(np.float32)  # 6xN -> Nx6
 
        for idx, class_id in enumerate(bbox[:, 5]):
            obj_name = self.names[int(bbox[idx][5])]
            if not obj_name in self.object_list:
                continue
            confidence = bbox[idx][4]
            if float(confidence) < self.conf_threshold:
                continue
            x1 = int(bbox[idx][0])
            y1 = int(bbox[idx][1])
            x2 = int(bbox[idx][2])
            y2 = int(bbox[idx][3])
 
            pred_boxes.append([x1, y1, x2, y2, obj_name, confidence]) #获取推理结果
 
        return pred_boxes

父主题： 代码实现

目标追踪逻辑代码

在实现目标检测的前提下，结合小车的基础控制部分，将小车的速度调整依赖到目标检测的推理结果上，就能实现目标追踪。

“python/src/scenes/tracking.py”为目标追踪的核心代码，示例代码定义追踪的运行逻辑。

class Tracking(BaseScene):
    def __init__(self, memory_name, camera_info, msg_queue):
        super().__init__(memory_name, camera_info, msg_queue)
        self.model = None
 
    def init_state(self):
        log.info(f'start init &#123;self.__class__.__name__&#125;')
        model_path = os.path.join(os.getcwd(), 'weights', 'tracking.om')
        if not os.path.exists(model_path):
            log.error(f'Cannot find the offline inference model(.om) file needed for &#123;self.__class__.__name__&#125;  scene.')
            return True
        self.model = YoloV5(model_path) #加载模型
        log.info(f'&#123;self.__class__.__name__&#125; model init succ.')
        self.ctrl.execute(SetServo(servo=[90, 65])) #设置舵机角度
        return False
 
    def loop(self):
        ret = self.init_state() #执行初始化
        if ret:
            log.error(f'&#123;self.__class__.__name__&#125; init failed.')
            return
        frame = np.ndarray((self.height, self.width, 3), dtype=np.uint8, buffer=self.broadcaster.buf) #获取共享内存中的图片
        log.info(f'&#123;self.__class__.__name__&#125; loop start')
        last_action = None
        last_not_seen = True
        forward_speed_slow = 30
        forward_speed_fast = 40
        while True:
            action = None
            if self.stop_sign.value:
                break
            if self.pause_sign.value:
                continue
 
            img_bgr = frame.copy()
            bboxes = self.model.infer(img_bgr)
            log.info(f'&#123;bboxes&#125;')
            if not bboxes:
                if last_not_seen:
                    action = Stop()
                else:
                    last_not_seen = True
                    continue
            else:
                if len(bboxes) > 1:
                    ori_box = sorted(bboxes, key=lambda x: x[-1], reverse=True)[0][:4]
                else:
                    ori_box = bboxes[0][:4]
                x1, y1, x2, y2 = ori_box
                x, y = (x1 + x2) // 2, (y1 + y2) // 2 #计算目标中心点的x与y坐标
                h, w = y2 - y1, x2 - x1 #计算目标的宽高
 
                if h * w < 141 * 128 or y < 110: #进行距离判断，如果过远就加速，否则减速
                    speed = forward_speed_fast
                else:
                    speed = forward_speed_slow
 
                if x < 400:
                    action = TurnLeft(degree=1.1, speed=speed) #左转
                elif x > 1000:
                    action = TurnRight(degree=1.1, speed=speed) #右转
                else:
                    action = Advance(speed=speed) #直行
 
                if h * w > 800 * 500 or y > 390: #如果距离过近则停车
                    action = Stop()
 
            if action is None or action == last_action:
                continue
            self.ctrl.execute(action)
            last_action = action

初始化并导入Yolov5目标检测模型。

def __init__(self, memory_name, camera_info, msg_queue):
        super().__init__(memory_name, camera_info, msg_queue)
        self.model = None
 
    def init_state(self):
        log.info(f'start init &#123;self.__class__.__name__&#125;')
        model_path = os.path.join(os.getcwd(), 'weights', 'tracking.om')
        if not os.path.exists(model_path):
            log.error(f'Cannot find the offline inference model(.om) file needed for &#123;self.__class__.__name__&#125;  scene.')
            return True
        self.model = YoloV5(model_path)
        log.info(f'&#123;self.__class__.__name__&#125; model init succ.')
        self.ctrl.execute(SetServo(servo=[90, 65]))
        return False

在得到正确导入结果后，开启循环，不断获取推理结果，并根据结果估算智能小车和追踪目标之间的距离，再根据计算出的结果下达不同的运动指令，设置慢速和快速跟进的两个速度。

def loop(self):
        ret = self.init_state()
        if ret:
            log.error(f'&#123;self.__class__.__name__&#125; init failed.')
            return
        frame = np.ndarray((self.height, self.width, 3), dtype=np.uint8, buffer=self.broadcaster.buf)
        log.info(f'&#123;self.__class__.__name__&#125; loop start')
        last_action = None
        last_not_seen = True
        forward_speed_slow = 30
        forward_speed_fast = 40

获取推理结果的外接框。
```
bboxes = self.model.infer(img_bgr)
```

计算出目标框的中心点的位置和目标框的宽高大小。

log.info(f'&#123;bboxes&#125;')
            if not bboxes:
                if last_not_seen:
                    action = Stop()
                else:
                    last_not_seen = True
                    continue
            else:
                if len(bboxes) > 1:
                    ori_box = sorted(bboxes, key=lambda x: x[-1], reverse=True)[0][:4]
                else:
                    ori_box = bboxes[0][:4]
                x1, y1, x2, y2 = ori_box
                x, y = (x1 + x2) // 2, (y1 + y2) // 2
                h, w = y2 - y1, x2 - x1

根据计算出的目标框的大小来判断小车和目标之间的距离，再调整小车的行进速度。根据目标近大远小的简单规则，存在两个判断条件，如果目标框的面积小于一定值，就说明小车与目标距离较远，需要快速接近目标，另外如果识别框的中心点的纵坐标大于0，也就是在摄像头视角里的上半部分，也说明小车距离目标较远，也需快速接近目标，反之亦然。

if h * w < 141 * 128 or y < 110:
                    speed = forward_speed_fast
                else:
                    speed = forward_speed_slow

另外如果前方目标在小车的偏左或偏右的位置，也可以采用同样的判断方法，即判断目标框的中心点的横坐标落在小车摄像头视角画面中的左侧还是右侧，进而下发对应的微调转向的命令，实现跟踪目标的方向调整。

if x < 400:
                    action = TurnLeft(degree=1.1, speed=speed)
                elif x > 1000:
                    action = TurnRight(degree=1.1, speed=speed)
                else:
                    action = Advance(speed=speed)
 
                if h * w > 800 * 500 or y > 390:
                    action = Stop()

父主题： 代码实现