20 基于 OpenCV 的颜色追踪

基于 OpenCV 的颜色追踪

在本章教程中我们会在 OpenCV 的相关功能中加入一些控制外设的函数，例如，在本章教程中，摄像头云台会转动，确保你的手或其它易碎物品远离摄像头云台的转动半径。

准备工作

由于产品开机默认会自动运行主程序，主程序会占用摄像头资源，这种情况下是不能使用本教程的，需要结束主程序或禁止主程序自动运行后再重新启动机器人。

这里需要注意的是，由于机器人主程序中使用了多线程且由 crontab 配置开机自动运行，所以常规的 sudo killall python 的方法通常是不起作用的，所以我们这里介绍禁用主程序自动运行的方法。

如果你已经禁用了机器人主程序的开机自动运行，则不需要执行下面的结束主程序章节。

结束主程序

1. 点击上方本页面选项卡旁边的 “+”号，会打开一个新的名为 Launcher 的选项卡。

2. 点击 Other 内的 Terminal，打开终端窗口。

3. 在终端窗口内输入 bash 后按回车。

4. 现在你可以使用 Bash Shell 来控制机器人了。

5. 输入命令： crontab -e

6. 如果询问希望使用什么编辑器，输入 1 后按回车，选择使用 nano。

7. 打开 crontab 的配置文件后，你可以看到以下两行内容

@reboot ~/ugv_pt_rpi/ugv-env/bin/python ~/ugv_pt_rpi/app.py >> ~/ugv.log 2>&1
@reboot /bin/bash ~/ugv_pt_rpi/start_jupyter.sh >> ~/jupyter_log.log 2>&1

8.在 ……app.py >> …… 这行的最前面添加一个 # 号来注释掉这行。

# @reboot ~/ugv_pt_rpi/ugv-env/bin/python ~/ugv_pt_rpi/app.py >> ~/ugv.log 2>&1
@reboot /bin/bash ~/ugv_pt_rpi/start_jupyter.sh >> ~/jupyter_log.log 2>&1

9. 在终端页面，按 Ctrl + X 退出，它会询问你 Save modified buffer? 输入 Y，按回车，保存变更。

10. 重启设备，注意该过程会暂时关闭当前的 jupyter Lab，如果你上一步没有注释掉 ……start_jupyter.sh >>…… 这一行，那么当机器人重新开机后，你仍然可以正常使用 jupyter Lab (JupyterLab 与 机器人主程序 app.py 是互相独立运行的)，可能需要重新刷新页面。

11. 这里需要注意一点，由于下位机持续通过串口与上位机通信，上位机在重启过程中有可能会由于串口电平的连续变化不能正常开机，拿上位机为树莓派的情况举例，重启时树莓派关机后不会再开机，红灯常亮绿灯不亮，此时可以关闭机器人电源开关，再打开，机器人就能够正常重启了。

12. 输入重启命令： sudo reboot

13. 等待设备重启后（重启过程中树莓派的绿灯会闪烁，当绿灯闪烁频率降低或灭掉后即代表已经启动成功），刷新页面，继续该教程的剩余部分。

例程

以下代码块可以直接运行：

1. 选中下面的代码块

2. 按 Shift + Enter 运行代码块

3. 观看实时视频窗口

4. 按 STOP 关闭实时视频，释放摄像头资源

如果运行时不能看到摄像头实时画面

• 需要点击上方的 Kernel - Shut down all kernels
• 关闭本章节选项卡，再次打开
• 点击 STOP 释放摄像头资源后重新运行代码块
• 重启设备

运行

在本章教程中，摄像头云台会转动，确保你的手或其它易碎物品远离摄像头云台的转动半径。

我们在例程中默认检测蓝色小球，确保画面背景中没有蓝色物体影响颜色识别功能，你也可以通过二次开发来更改检测颜色（HSV色彩空间）。

import matplotlib.pyplot as plt
import cv2
from picamera2 import Picamera2
import numpy as np
from IPython.display import display, Image
import ipywidgets as widgets
import threading

# Stop button
# ================
stopButton = widgets.ToggleButton(
    value=False,
    description='Stop',
    disabled=False,
    button_style='danger', # 'success', 'info', 'warning', 'danger' or ''
    tooltip='Description',
    icon='square' # (FontAwesome names without the `fa-` prefix)
)


def gimbal_track(fx, fy, gx, gy, iterate):
    global gimbal_x, gimbal_y
    distance = math.sqrt((fx - gx) ** 2 + (gy - fy) ** 2)
    gimbal_x += (gx - fx) * iterate
    gimbal_y += (fy - gy) * iterate
    if gimbal_x > 180:
        gimbal_x = 180
    elif gimbal_x < -180:
        gimbal_x = -180
    if gimbal_y > 90:
        gimbal_y = 90
    elif gimbal_y < -30:
        gimbal_y = -30
    gimbal_spd = int(distance * track_spd_rate)
    gimbal_acc = int(distance * track_acc_rate)
    if gimbal_acc < 1:
        gimbal_acc = 1
    if gimbal_spd < 1:
        gimbal_spd = 1
    base.base_json_ctrl({"T":self.CMD_GIMBAL,"X":gimbal_x,"Y":gimbal_y,"SPD":gimbal_spd,"ACC":gimbal_acc})
    return distance


# Display function
# ================
def view(button):
    picam2 = Picamera2()
    picam2.configure(picam2.create_video_configuration(main={"format": 'XRGB8888', "size": (640, 480)}))
    picam2.start()
    display_handle=display(None, display_id=True)

    color_upper = np.array([120, 255, 220])
    color_lower = np.array([ 90, 120,  90])
    min_radius = 12
    track_color_iterate = 0.023
    
    while True:
        frame = picam2.capture_array()
        # frame = cv2.flip(frame, 1) # if your camera reverses your image

        # uncomment this line if you are using USB camera
        # frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)

        img = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)
        blurred = cv2.GaussianBlur(img, (11, 11), 0)
        hsv = cv2.cvtColor(blurred, cv2.COLOR_BGR2HSV)
        mask = cv2.inRange(hsv, color_lower, color_upper)
        mask = cv2.erode(mask, None, iterations=5)
        mask = cv2.dilate(mask, None, iterations=5)

        cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
            cv2.CHAIN_APPROX_SIMPLE)
        cnts = imutils.grab_contours(cnts)
        center = None

        height, width = img.shape[:2]
        center_x, center_y = width // 2, height // 2

        if len(cnts) > 0:
            # find the largest contour in the mask, then use
            # it to compute the minimum enclosing circle and
            # centroid
            c = max(cnts, key=cv2.contourArea)
            ((x, y), radius) = cv2.minEnclosingCircle(c)
            M = cv2.moments(c)
            center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))

            # only proceed if the radius meets a minimum size
            if radius > min_radius:
                distance = gimbal_track(center_x, center_y, center[0], center[1], track_color_iterate) #
                cv2.circle(overlay_buffer, (int(x), int(y)), int(radius), (128, 255, 255), 1)
        
        
        _, frame = cv2.imencode('.jpeg', frame)
        display_handle.update(Image(data=frame.tobytes()))
        if stopButton.value==True:
            picam2.close()
            display_handle.update(None)
            
            
# Run
# ================
display(stopButton)
thread = threading.Thread(target=view, args=(stopButton,))
thread.start()