Welcome to DOFBOT-Ultra repository

3. Color sorting

Color Sorting

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

For all functions involving grasping of square blocks, please use 30x30x30 mm wooden blocks. Non-standard sized objects may cause servo No. 6 stall and damage.

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!!!

The color sorting function uses HSV color recognition to detect multiple blocks of different colors in the recognition area, then grabs and places them in the corresponding color areas sequentially according to the user-selected color order.

Note: Before starting the program, please follow the [Assembly Tutorial] -> [Install the Map] tutorial to correctly install the map before operation.

1. Startup and Operation

Start ROS Node Service

Open the system terminal and enter the following command. If already running, there is no need to start again.


xxxxxxxxxx
ros2 run dofbot_ultra_info kinematics_dofbotultra

Start the Program

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.

Code path:


xxxxxxxxxx
dofbot_color_sorting/scripts/Color_Sorting.ipynb

Click the Run All button on the JupyterLab toolbar, then scroll to the middle color calibration section.

202309010001

Operation Steps

Drag the sliders to calibrate colors. After calibration, click Save HSV, then click Exit Calibration (required).

Note: When calibrating, the ideal situation is that the left frame only covers the top surface of the corresponding color block (preferably not including the sides, otherwise grasping may have offset)

Wait until the image stops being captured (camera released), scroll to the bottom, select the last cell with the display function, and click Run.

If the camera cannot be opened after clicking "Start Detection", please restart the device and re-execute the above steps

Here I choose to filter out the green block and grab red, yellow, and blue blocks in order

Click Grasp

For standard USB cameras, grasping accuracy is better than depth cameras because the conversion from pixel coordinates to world coordinates is simpler, resulting in fewer error sources and higher grasping precision.

2. Technical Overview

The core workflow of color sorting:

Camera Calibration and Coordinate Mapping: Use calibrated camera intrinsics for automatic distortion correction, and map pixel coordinates to real-world coordinates via the six-point calibration homography matrix
Color Detection: On the distortion-corrected image, use HSV color space for color recognition and position localization of each block
Sequential Arrangement: Determine the grab and place order based on the user-selected color sequence
Arm Sorting: Grab blocks sequentially and place them in the corresponding color areas on the map

3. Core Code Analysis

Import header files


xxxxxxxxxx
import cv2 as cv
import threading
from time import sleep
import ipywidgets as widgets
from IPython.display import display
from identify_target import identify_GetTarget
from dofbot_utils.dofbot_config import read_HSV, write_HSV
from dofbot_utils.fps import FPS

Create instances and initialize parameters


xxxxxxxxxx
target = identify_GetTarget()
msg = {}
model = "General"
xy = [90, 120]
color_list = {'1': 'red', '2': 'green', '3': 'blue', '4': 'yellow'}
color_hsv = {
    "red":    ((0, 122, 100), (14, 184, 217)),
    "green":  ((36, 52, 100), (59, 122, 255)),
    "blue":   ((92, 153, 125), (124, 255, 255)),
    "yellow": ((20, 163, 168), (35, 236, 255)),
}
HSV_path = "/home/yahboom/dofbot_ultra_ws/src/dofbot_color_identify/scripts/HSV_config.txt"
try:
    read_HSV(HSV_path, color_hsv)
    print("HSV loaded from file:", HSV_path)
except Exception as e:
    print(f"Read HSV_config Error: {e}, using built-in defaults")


xxxxxxxxxx
import Arm_Lib
arm = Arm_Lib.Arm_Device()
joints_0 = [xy[0], xy[1], 0, 0, 90, 30]
arm.Arm_serial_servo_write6_array(joints_0, 1000)
fps = FPS()

Create widgets


xxxxxxxxxx
button_layout = widgets.Layout(width='320px', height='60px', align_self='center')
output = widgets.Output()
# Observation position sliders
joint1_slider = widgets.IntSlider(description='joint1:', value=xy[0], min=70, max=110, step=1, orientation='horizontal')
joint2_slider = widgets.IntSlider(description='joint2:', value=xy[1], min=90, max=150, step=1, orientation='horizontal')
# Select grab color order
color_list_one   = widgets.Dropdown(options=['red', 'green', 'blue', 'yellow', 'none'], value='red', disabled=False)
color_list_two   = widgets.Dropdown(options=['red', 'green', 'blue', 'yellow', 'none'], value='green', disabled=False)
color_list_three = widgets.Dropdown(options=['red', 'green', 'blue', 'yellow', 'none'], value='blue', disabled=False)
color_list_four  = widgets.Dropdown(options=['red', 'green', 'blue', 'yellow', 'none'], value='yellow', disabled=False)
# Buttons
target_detection = widgets.Button(description='Start Detection', button_style='info', layout=button_layout)
reset_color_list = widgets.Button(description='Reset Colors', button_style='info', layout=button_layout)
grap = widgets.Button(description='Grasp', button_style='success', layout=button_layout)
exit_button = widgets.Button(description='Exit', button_style='danger', layout=button_layout)
# Layout
imgbox = widgets.Image(format='jpg', height=480, width=640, layout=widgets.Layout(align_self='center'))
color_down = widgets.HBox([exit_button, reset_color_list], layout=widgets.Layout(align_self='center'))
color_img = widgets.VBox([imgbox, color_down], layout=widgets.Layout(align_self='center'))
color_identify = widgets.VBox(
    [joint1_slider, joint2_slider,
     color_list_one, color_list_two, color_list_three, color_list_four,
     target_detection, grap],
    layout=widgets.Layout(align_self='center'))
controls_box = widgets.HBox([color_img, color_identify], layout=widgets.Layout(align_self='center'))

Color selection sequence


xxxxxxxxxx
def color_list_one_Callback(value):
    global model,color_list
    model="General"
    if not isinstance(value['new'],str):return
    if value['new'] == "none":
        if '1' in color_list:del color_list['1']
    elif value['new'] == "red":
        color_list['1'] = "red"
    elif value['new']== "green":
        color_list['1'] = "green"
    elif value['new'] == "blue":
        color_list['1'] = "blue"
    elif value['new'] == "yellow":
        color_list['1'] = "yellow"
    with output:
        print("color_list_three_Callback clicked.",color_list)
# ... color_list_two/three/four callbacks similar
color_list_one.observe(color_list_one_Callback)
color_list_two.observe(color_list_two_Callback)
color_list_three.observe(color_list_three_Callback)
color_list_four.observe(color_list_four_Callback)

Main program


xxxxxxxxxx
def camera():
    global color_hsv, model, msg, color_list, output
    from dofbot_utils.camera import create_camera
    capture = create_camera()
    while capture.isOpened():
        try:
            _, img = capture.read()
            img = cv.resize(img, (640, 480))
            fps.update_fps()
            xy = [joint1_slider.value, joint2_slider.value]
            target.xy = xy
            if len(color_list) != 0 and model in ('Detection', 'Grap'):
                img, msg_new = target.select_color(img, color_hsv, color_list)
                msg = msg_new
            if model == 'Reset_list':
                msg = {}
                color_list = {}
                color_list_one.value = 'none'
                color_list_two.value = 'none'
                color_list_three.value = 'none'
                color_list_four.value = 'none'
                model = 'General'
            if model == 'Grap':
                if len(msg) != 0:
                    print("grasp msg:", msg)
                    threading.Thread(target=target.target_run, args=(msg, xy)).start()
                    msg = {}
                    model = 'Detection'
            if model == 'Exit':
                capture.release()
                break
            fps.show_fps(img)
            imgbox.value = cv.imencode('.jpg', img)[1].tobytes()
        except KeyboardInterrupt:
            capture.release()
            break

Startup


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

Catch game

Color stacking