Welcome to DOFBOT-Ultra repository

Overall Detection

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 MediaPipe Holistic to simultaneously detect facial mesh, body pose, and left/right hand keypoints, drawing all detection results on both the original image and a black background.

1、Startup

1.1、Startup

• Enter the following command to start the program

xxxxxxxxxx
ros2 run dofbot_mediapipe 03_Holistic.py

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

1.2、Source Code

Raspberry Pi source path:

xxxxxxxxxx
~/dofbot_ultra_ws/src/dofbot_mediapipe/scripts/03_Holistic.py

RDK X5 source path:

xxxxxxxxxx
~/dofbot_ultra_ws/src/2d/dofbot_mediapipe/scripts/03_Holistic.py

#!/usr/bin/env python3
# encoding: utf-8
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 HolisticDetector:
    def __init__(self, staticMode=False, landmarks=True,
                 detectionCon=0.5, trackingCon=0.5):
        self.mpHolistic = mp.solutions.holistic
        self.mpFaceMesh = mp.solutions.face_mesh
        self.mpHands = mp.solutions.hands
        self.mpPose = mp.solutions.pose
        self.mpDraw = mp.solutions.drawing_utils
        self.holistic = self.mpHolistic.Holistic(
            static_image_mode=staticMode,
            smooth_landmarks=landmarks,
            min_detection_confidence=detectionCon,
            min_tracking_confidence=trackingCon)
        self.lmDrawSpec = mp.solutions.drawing_utils.DrawingSpec(
            color=(0, 0, 255), thickness=-1, circle_radius=3)
        self.drawSpec = mp.solutions.drawing_utils.DrawingSpec(
            color=(0, 255, 0), thickness=2, circle_radius=2)

    def _draw_face(self, target, landmarks):
        try:
            self.mpDraw.draw_landmarks(
                target, landmarks,
                self.mpFaceMesh.FACEMESH_CONTOURS,
                self.lmDrawSpec, self.drawSpec)
        except Exception:
            pass

    def detect(self, frame, draw=True):
        img = np.zeros(frame.shape, np.uint8)
        img_RGB = cv.cvtColor(frame, cv.COLOR_BGR2RGB)
        results = self.holistic.process(img_RGB)

        if results.face_landmarks:
            if draw:
                self._draw_face(frame, results.face_landmarks)
            self._draw_face(img, results.face_landmarks)
        if results.pose_landmarks:
            if draw:
                self.mpDraw.draw_landmarks(
                    frame, results.pose_landmarks,
                    self.mpPose.POSE_CONNECTIONS,
                    self.lmDrawSpec, self.drawSpec)
            self.mpDraw.draw_landmarks(
                img, results.pose_landmarks,
                self.mpPose.POSE_CONNECTIONS,
                self.lmDrawSpec, self.drawSpec)
        if results.left_hand_landmarks:
            if draw:
                self.mpDraw.draw_landmarks(
                    frame, results.left_hand_landmarks,
                    self.mpHands.HAND_CONNECTIONS,
                    self.lmDrawSpec, self.drawSpec)
            self.mpDraw.draw_landmarks(
                img, results.left_hand_landmarks,
                self.mpHands.HAND_CONNECTIONS,
                self.lmDrawSpec, self.drawSpec)
        if results.right_hand_landmarks:
            if draw:
                self.mpDraw.draw_landmarks(
                    frame, results.right_hand_landmarks,
                    self.mpHands.HAND_CONNECTIONS,
                    self.lmDrawSpec, self.drawSpec)
            self.mpDraw.draw_landmarks(
                img, results.right_hand_landmarks,
                self.mpHands.HAND_CONNECTIONS,
                self.lmDrawSpec, self.drawSpec)
        return frame, img


def frame_combine(frame, src):
    if len(frame.shape) == 3:
        frameH, frameW = frame.shape[:2]
        srcH, srcW = src.shape[:2]
        dst = np.zeros((max(frameH, srcH), frameW + srcW, 3), np.uint8)
        dst[:frameH, :frameW] = frame
        dst[:srcH, frameW:frameW + srcW] = src
    else:
        src = cv.cvtColor(src, cv.COLOR_BGR2GRAY)
        frameH, frameW = frame.shape[:2]
        srcH, srcW = src.shape[:2]
        dst = np.zeros((max(frameH, srcH), frameW + srcW), np.uint8)
        dst[:frameH, :frameW] = frame
        dst[:srcH, frameW:frameW + srcW] = src
    return dst


def main():
    detector = HolisticDetector()
    fps = FPS()

    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

            frame, img = detector.detect(frame)
            fps.update_fps()
            fps.show_fps(frame)

            combined = frame_combine(frame, img)
            cv.imshow('HolisticDetector', combined)

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


if __name__ == '__main__':
    main()