Welcome to DOFBOT-Ultra repository

2. Color block tracking

Color Block Tracking Experiment

If using a depth camera (DABAI_DCW2), make sure to run ros2 run dofbot_ultra_driver calibrate_6point for a six-point calibration before running this example. Otherwise, the arm cannot accurately locate the target.

Please ensure that the web control program backend process is not running in the background. To close it, refer to the [Get Started Quickly - 2. Access the Terminal] section in [Disable the Auto-Start Web Control Backend Process]. After closing, please restart DofbotUltra!!!

This tutorial is an advanced version of the color block positioning experiment, adding arm tracking functionality. After identifying the target color block, the arm automatically tracks the block's movement to keep the target centered in the frame.

If color detection fails, run HSV_calibration.ipynb in the same directory to calibrate the colors first.

1. Startup and Operation

Open a browser (any browser on any host in the same local network) and enter the IP address:8888 in the address bar to access JupyterLab.

For first-time JupyterLab configuration, refer to section [3. Remote Control Course --> 3. PC Web Page Programming].

Code path:


xxxxxxxxxx
dofbot_color_follow/scripts/Color_follow.ipynb

Click the Run All button on the JupyterLab toolbar, then scroll to the bottom.

202309010001

Color Tracking

Tracking is disabled by default. Select the corresponding function button to enable it.

Start the code block and click the [color_follow] button to enable color tracking. You can switch colors below the image. Place a block in view. Once the camera identifies it, the arm tracks the block's movement, facing the center of the block.

Click the [follow_cancel] button to cancel tracking.

Click the [Exit] button to exit the program.

Learning Tracking

(1) Start the code block and click the [learning_color] button. A box appears at the center of the screen, as shown in the left image.

(2) Place an object inside the box. The HSV high/low thresholds are printed in real time. When [OK !!!] appears below the box, recognition is successful.

(3) Click the [learning_follow] button. As long as the learned color contour is detected, real-time tracking will begin.

(4) Click the [follow_cancel] button to cancel tracking.

Click the [Exit] button to exit the program.

2. Technical Overview

Target tracking includes two modes: Color Tracking and Learning Tracking.

Core principles:

Target Detection: Filter using HSV color space thresholds to identify the target color block and get its center coordinates in the frame.
Deviation Calculation: Calculate the deviation between the target center and the image center (X and Y directions).
PID Control: Convert the deviation into control values for the arm servos using a PID algorithm for smooth and stable motion.
Arm Drive: Drive the arm to move so the target center aligns with the image center.

Learning tracking adds a color learning feature: select the target area at the center of the screen, and the system automatically extracts the HSV high/low thresholds to track objects of any color.

3. Core Code Analysis

Import header files.


xxxxxxxxxx
import cv2 as cv
import threading
import random
from time import sleep
import ipywidgets as widgets
from IPython.display import display
from color_follow import color_follow
from dofbot_utils.fps import FPS
from dofbot_utils.robot_controller import Robot_Controller
from dofbot_utils.dofbot_config import *
from dofbot_utils.camera import create_camera

Create instances and initialize parameters.


xxxxxxxxxx
robot = Robot_Controller()
robot.move_init_pose()
fps = FPS()
follow = color_follow()
model = 'General'
HSV_learning = ((0, 240, 54), (8, 255, 255))
color_hsv = {"red": ((0, 25, 90), (10, 255, 255)),
             "green": ((53, 36, 40), (80, 255, 255)),
             "blue": ((110, 80, 90), (120, 255, 255)),
             "yellow": ((25, 20, 55), (50, 255, 255))}
color = [[random.randint(0, 255) for _ in range(3)] for _ in range(255)]
HSV_path="/home/yahboom/dofbot_ultra_ws/src/dofbot_color_follow/scripts/HSV_config.txt"
try: read_HSV(HSV_path,color_hsv)
except Exception: print("Read HSV_config Error !!!")

Create widgets.


xxxxxxxxxx
button_layout = widgets.Layout(width='200px', height='100px', align_self='center')
output = widgets.Output()
color_follow = widgets.Button(description='color_follow', button_style='success', layout=button_layout)
choose_color = widgets.ToggleButtons(options=['red', 'green', 'blue', 'yellow'], button_style='success',
             tooltips=['Description of slow', 'Description of regular', 'Description of fast'])
follow_cancel = widgets.Button(description='follow_cancel', button_style='danger', layout=button_layout)
learning_color = widgets.Button(description='learning_color', button_style='primary', layout=button_layout)
learning_follow = widgets.Button(description='learning_follow', button_style='success', layout=button_layout)
exit_button = widgets.Button(description='Exit', button_style='danger', layout=button_layout)
imgbox = widgets.Image(format='jpg', height=480, width=640, layout=widgets.Layout(align_self='auto'))
img_box = widgets.VBox([imgbox, choose_color], layout=widgets.Layout(align_self='auto'))
Slider_box = widgets.VBox([color_follow, learning_color, learning_follow,follow_cancel,exit_button],
                          layout=widgets.Layout(align_self='auto'))
controls_box = widgets.HBox([img_box, Slider_box], layout=widgets.Layout(align_self='auto'))

Mode switching.


xxxxxxxxxx
def color_follow_Callback(value):
    global model
    model = 'color_follow'
def learning_color_Callback(value):
    global model
    model = 'learning_color'
def learning_follow_Callback(value):
    global model
    model = 'learning_follow'
def follow_cancel_Callback(value):
    global model
    model = 'General'
    robot.move_init_pose()
def exit_button_Callback(value):
    global model
    model = 'Exit'
color_follow.on_click(color_follow_Callback)
learning_color.on_click(learning_color_Callback)
learning_follow.on_click(learning_follow_Callback)
follow_cancel.on_click(follow_cancel_Callback)
exit_button.on_click(exit_button_Callback)

Main program.


xxxxxxxxxx
def camera():
    global HSV_learning,model
    capture = create_camera()
    while capture.isOpened():
        try:
            _, img = capture.read()
            fps.update_fps()
            if model == 'color_follow':
                img = follow.follow_function(img, color_hsv[choose_color.value])
                cv.putText(img, choose_color.value, (int(img.shape[0] / 2), 50), cv.FONT_HERSHEY_SIMPLEX, 2, color[random.randint(0, 254)], 2)
            if model == 'learning_color':
                img,HSV_learning = follow.get_hsv(img)
            if model == 'learning_follow' :
                if len(HSV_learning)!=0:
                    print("HSV_learning", HSV_learning)
                    img = follow.learning_follow(img, HSV_learning)
                    cv.putText(img,'LeColor', (240, 50), cv.FONT_HERSHEY_SIMPLEX, 1, color[random.randint(0, 254)], 1)
            if model == 'Exit':
                cv.destroyAllWindows()
                capture.release()
                break
            fps.show_fps(img)
            imgbox.value = cv.imencode('.jpg', img)[1].tobytes()
        except KeyboardInterrupt:capture.release()

Startup.


display(controls_box,output)
threading.Thread(target=camera, ).start()

Color block positioning

Face positioning