20 Color Tracking Based on OpenCV

From Waveshare Wiki
Jump to: navigation, search

Color Tracking with OpenCV

In this chapter, we will add some functions for controlling peripherals to the relevant OpenCV functionality. For example, in this chapter, the camera gimbal will rotate. Make sure your hands or other fragile items stay away from the rotation radius of the camera gimbal.

Preparation

Since the product runs the main program automatically at startup by default, which occupies the camera resource, you cannot use this tutorial under that condition. You need to terminate the main program or disable its auto-start, then restart the robot.

Note that the robot's main program uses multi‑threading and is configured to run at boot via crontab, so a conventional sudo killall python usually does not work. Therefore we describe here how to disable the auto-start of the main program.

If you have already disabled the auto-start of the robot's main program, you do not need to execute the Terminate the Main Program section below.

Terminate the Main Program

1. Click the "+" icon next to the current page tab to open a new Launcher tab.

2. Click "Terminal" under "Other" to open a terminal window.

3. In the terminal window, type bash and press Enter.

4. You can now control the robot using the Bash shell.

5. Enter the command: crontab -e

6. If asked which editor to use, type 1 and press Enter to select nano.

7. After opening the crontab configuration file, you should see the following two lines: 

@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. Add a # at the very beginning of the line that starts with ……app.py >> …… to comment it out. 

# @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. In the terminal page, press Ctrl+X to exit. It will ask Save modified buffer? Type Y and press Enter to save the changes.

10. Reboot the device. Note that this process will temporarily close the current Jupyter Lab session. If you did not comment out the line ……start_jupyter.sh >> …… in the previous step, you will still be able to use Jupyter Lab normally after the robot restarts (JupyterLab and the robot main program app.py run independently). You may need to refresh the page.

11. One important point: because the lower computer continuously communicates with the upper computer via the serial port, a voltage glitch on the serial line during the upper computer reboot may prevent it from booting correctly. For example, on a Raspberry Pi as the upper computer, after a reboot the Pi may shut down but not restart – the red LED stays on while the green LED does not light. In that case, you can turn off the robot power switch and then turn it on again; the robot will then restart normally.

12. Enter the reboot command: sudo reboot

13. Wait for the device to restart (during reboot the green LED on the Raspberry Pi will blink; when the blinking slows down or stops, it indicates that startup has succeeded), refresh the page, and continue with the remaining parts of this tutorial.

Example

The following code block can be executed directly:

1. Select the code block below.

2. Press Shift+Enter to run the code block.

3. Watch the real-time video window.

4. Press STOP to close the real-time video and release the camera resource.

If you cannot see the camera's real-time video when running

  • Click "Kernel" → "Shut down all kernels"
  • Close this chapter's tab and reopen it
  • Press STOP to release the camera resource, then re‑run the code block
  • Reboot the device

Operation

In this chapter, the camera gimbal will rotate. Make sure your hands or other fragile items stay away from the rotation radius of the camera gimbal.

In the example, we detect a blue ball by default. Make sure there are no blue objects in the background that could interfere with color recognition. You can also change the detection color (HSV color space) through further development. 

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()
Retrieved from "https://www.waveshare.com/w/index.php?title=20_Color_Tracking_Based_on_OpenCV&oldid=110522"