Welcome to DOFBOT-Ultra repository

Paintbrush

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

Use finger gestures to perform virtual painting on the camera feed. Supports multiple colors and an eraser function.

1、Operation Instructions

1.1、Gesture Description

1.2、Available Colors

There are 5 color blocks at the top of the screen. In selection mode, move your finger to the corresponding area to switch:

1.3、Startup

• Enter the following command to start the program

xxxxxxxxxx
ros2 run dofbot_mediapipe 08_VirtualPaint.py

Press the q key in the image window or Ctrl+C in the terminal to exit the program.

1.4、Source Code

Raspberry Pi source path:

xxxxxxxxxx
~/dofbot_ultra_ws/src/dofbot_mediapipe/scripts/08_VirtualPaint.py

RDK X5 source path:

xxxxxxxxxx
~/dofbot_ultra_ws/src/2d/dofbot_mediapipe/scripts/08_VirtualPaint.py

#!/usr/bin/env python3
# encoding: utf-8
import math
import cv2 as cv
import numpy as np
import mediapipe as mp
from dofbot_utils.camera import create_camera
from dofbot_utils.fps import FPS


class HandDetector:
    def __init__(self, mode=False, maxHands=2, detectorCon=0.5, trackCon=0.5):
        self.tipIds = [4, 8, 12, 16, 20]
        self.mpHand = mp.solutions.hands
        self.mpDraw = mp.solutions.drawing_utils
        self.hands = self.mpHand.Hands(
            static_image_mode=mode,
            max_num_hands=maxHands,
            min_detection_confidence=detectorCon,
            min_tracking_confidence=trackCon)
        self.lmDrawSpec = mp.solutions.drawing_utils.DrawingSpec(
            color=(0, 0, 255), thickness=-1, circle_radius=15)
        self.drawSpec = mp.solutions.drawing_utils.DrawingSpec(
            color=(0, 255, 0), thickness=10, circle_radius=10)
        self.lmList = []

    def findHands(self, frame, draw=True):
        self.lmList = []
        img_RGB = cv.cvtColor(frame, cv.COLOR_BGR2RGB)
        self.results = self.hands.process(img_RGB)
        if self.results.multi_hand_landmarks:
            for handLms in self.results.multi_hand_landmarks:
                if draw:
                    self.mpDraw.draw_landmarks(
                        frame, handLms, self.mpHand.HAND_CONNECTIONS,
                        self.lmDrawSpec, self.drawSpec)
            for id, lm in enumerate(
                    self.results.multi_hand_landmarks[0].landmark):
                h, w, c = frame.shape
                cx, cy = int(lm.x * w), int(lm.y * h)
                self.lmList.append([id, cx, cy])
        return frame, self.lmList

    def _get_dist(self, p1, p2):
        return math.sqrt((p1[0] - p2[0]) ** 2 + (p1[1] - p2[1]) ** 2)

    def _calc_angle(self, pt1, pt2, pt3):
        p1 = (self.lmList[pt1][1], self.lmList[pt1][2])
        p2 = (self.lmList[pt2][1], self.lmList[pt2][2])
        p3 = (self.lmList[pt3][1], self.lmList[pt3][2])
        a = self._get_dist(p1, p2)
        b = self._get_dist(p2, p3)
        c = self._get_dist(p1, p3)
        try:
            radian = math.acos((a**2 + b**2 - c**2) / (2 * a * b))
            return abs(radian / math.pi * 180)
        except (ValueError, ZeroDivisionError):
            return 0

    def fingersUp(self):
        fingers = []
        a1 = self._calc_angle(self.tipIds[0], self.tipIds[0] - 1,
                               self.tipIds[0] - 2)
        a2 = self._calc_angle(self.tipIds[0] - 1, self.tipIds[0] - 2,
                               self.tipIds[0] - 3)
        fingers.append(1 if a1 > 150.0 and a2 > 150.0 else 0)
        for id in range(1, 5):
            if self.lmList[self.tipIds[id]][2] < \
                    self.lmList[self.tipIds[id] - 2][2]:
                fingers.append(1)
            else:
                fingers.append(0)
        return fingers


def main():
    hand_detector = HandDetector(detectorCon=0.85)
    fps_counter = FPS()
    xp = yp = 0
    boxx = 0
    imgCanvas = np.zeros((480, 640, 3), np.uint8)
    brushThickness = 5
    eraserThickness = 100
    top_height = 50
    Color = "Red"
    ColorList = {
        'Red': (0, 0, 255), 'Green': (0, 255, 0),
        'Blue': (255, 0, 0), 'Yellow': (0, 255, 255),
        'Black': (0, 0, 0),
    }

    cap = create_camera()
    if not cap.isOpened():
        raise RuntimeError("Failed to open camera")

    try:
        while cap.isOpened():
            ret, frame = cap.read()
            if not ret:
                continue

            h, w, c = frame.shape
            frame, lmList = hand_detector.findHands(frame, draw=False)
            if len(lmList) != 0:
                x1, y1 = lmList[8][1:]
                x2, y2 = lmList[12][1:]
                fingers = hand_detector.fingersUp()
                if fingers[1] and fingers[2]:
                    if y1 < top_height:
                        if 0 < x1 < int(w / 5) - 1:
                            boxx = 0; Color = "Red"
                        elif int(w / 5) < x1 < int(w * 2 / 5) - 1:
                            boxx = int(w / 5); Color = "Green"
                        elif int(w * 2 / 5) < x1 < int(w * 3 / 5) - 1:
                            boxx = int(w * 2 / 5); Color = "Blue"
                        elif int(w * 3 / 5) < x1 < int(w * 4 / 5) - 1:
                            boxx = int(w * 3 / 5); Color = "Yellow"
                        elif int(w * 4 / 5) < x1 < w - 1:
                            boxx = int(w * 4 / 5); Color = "Black"
                    cv.rectangle(frame, (x1, y1 - 25), (x2, y2 + 25),
                                 ColorList[Color], cv.FILLED)
                    cv.rectangle(frame, (boxx, 0),
                                 (boxx + int(w / 5), top_height),
                                 ColorList[Color], cv.FILLED)
                    bw = int(w / 5)
                    for i, c_name in enumerate(ColorList):
                        cv.rectangle(frame, (i * bw + (2 if i > 0 else 0), 0),
                                     ((i + 1) * bw - 1, top_height),
                                     ColorList[c_name], 3)
                    xp = yp = 0
                elif (fingers[1] and not fingers[2] and
                      math.hypot(x2 - x1, y2 - y1) > 50):
                    if xp == 0 and yp == 0:
                        xp, yp = x1, y1
                    thickness = (eraserThickness if Color == 'Black'
                                 else brushThickness)
                    cv.line(frame, (xp, yp), (x1, y1),
                            ColorList[Color], thickness)
                    cv.line(imgCanvas, (xp, yp), (x1, y1),
                            ColorList[Color], thickness)
                    cv.circle(frame, (x1, y1), 15, ColorList[Color],
                              cv.FILLED)
                    xp, yp = x1, y1
                else:
                    xp = yp = 0

            imgGray = cv.cvtColor(imgCanvas, cv.COLOR_BGR2GRAY)
            _, imgInv = cv.threshold(imgGray, 50, 255, cv.THRESH_BINARY_INV)
            imgInv = cv.cvtColor(imgInv, cv.COLOR_GRAY2BGR)
            frame = cv.bitwise_and(frame, imgInv)
            frame = cv.bitwise_or(frame, imgCanvas)

            fps_counter.update_fps()
            h_f = frame.shape[0]
            cv.rectangle(frame, (20, h_f - 100), (50, h_f - 70),
                         ColorList[Color], cv.FILLED)
            cv.putText(frame, Color, (70, h_f - 75),
                       cv.FONT_HERSHEY_SIMPLEX, 0.9, (0, 0, 255), 1)
            fps_counter.show_fps(frame)
            cv.imshow('VirtualPaint', frame)

            if cv.waitKey(1) & 0xFF == ord('q'):
                break
    finally:
        cap.release()
        cv.destroyAllWindows()


if __name__ == '__main__':
    main()