Color Stacking

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.

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

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.

The color stacking function is built upon the color sorting function, modifying the placement method to stacking blocks. It grabs blocks sequentially according to the user-selected color order and stacks them.

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_stacking/scripts/Color_Stacking.ipynb

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

Operation Steps

Calibrate color HSV

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

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

Click Grasp, and the blocks will be stacked in order.

2. Technical Overview

Color stacking has the same recognition principle as color sorting, with the difference being in the placement method:

1. 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

2. Color Detection: Use HSV color space to recognize block colors and positions

3. Sequential Arrangement: Determine the grab order based on the user-selected color sequence

4. Stacking Placement: Unlike sorting, each block is stacked at the same position (green area) with increasing height per layer, achieving vertical stacking of blocks

3. Core Code Analysis

• Import header files

​x
import cv2 as cv
import threading
from time import sleep
import ipywidgets as widgets
from IPython.display import display

from dofbot_utils.dofbot_config import read_HSV, write_HSV
from stacking_target import stacking_GetTarget
from dofbot_utils.fps import FPS

• Create instances and initialize parameters

xxxxxxxxxx
target = stacking_GetTarget()
num = 0
xy = [90, 120]
msg = {}
debug_pos = False
model = "General"
color_list = {'1': 'red', '2': 'green', '3': 'blue', '4': 'yellow'}
color_hsv  = {"red"   : ((0, 43, 46), (10, 255, 255)),
              "green" : ((35, 43, 46), (77, 255, 255)),
              "blue"  : ((100, 43, 46), (124, 255, 255)),
              "yellow": ((26, 43, 46), (34, 255, 255))}
HSV_path="/home/yahboom/dofbot_ultra_ws/src/dofbot_color_identify/scripts/HSV_config.txt"
try: read_HSV(HSV_path,color_hsv)
except Exception: print("No HSV_config file!!!")

• Main program

xxxxxxxxxx
def camera():
    global color_hsv, model, msg, color_list, debug_pos, output
    from dofbot_utils.camera import create_camera
    capture = create_camera()
    while capture.isOpened():
        try:
            _, img = capture.read()
            fps.update_fps()
            if len(color_list) != 0 and (model == 'Detection' or model == 'Grap'):
                img, msg = target.select_color(img, color_hsv, color_list)
                if debug_pos:
                    debug_pos = False
                    print("detect msg:", msg)
            if model == 'Reset_list':
                msg = {}
                color_list = {}
                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 Exception as e:
            print("program end")
            print(e)
            capture.release()

• Startup

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