Welcome to ROSMASTER A1 PRO repository

Hand Detection

1. Content Description

This lesson implements acquiring the color image and using the MediaPipe framework to detect hand joint points. This section requires entering commands in the terminal, where the terminal is opened according to the mainboard type. This lesson uses the Raspberry Pi 5 as an example.

For the Raspberry Pi 5 mainboard, you need to open a terminal on the host machine, then enter the command to access the Docker container, and enter the commands mentioned in this lesson inside the terminal after entering the Docker container. For the tutorial on entering the Docker container from the host, please refer to 【Robot Configuration and Operation Guide】, section 【3.Enter Docker (For Raspberry Pi)】 in this product tutorial.

For the Orin and RDK mainboards, simply open a terminal and enter the commands mentioned in this lesson.

2. Program Startup

The Raspberry Pi PI5 controller needs to enter the Docker container first; RDK X5 and Orin controllers do not need to.

To enter the Docker container (for steps, refer to 【Docker Course】 --- 【4. Docker Startup Script】),

All the docker terminals for the commands below need to enter the same Docker container before executing （for steps, refer to 【Docker Course】 --- 【3.Docker Submission and Multi-Terminal Access】）

First, enter the following command in the terminal to start the robot camera and low-level driver environment:

xxxxxxxxxx
ros2 launch dofbot_pro_driver A1pro_camera_arm.launch.py

After the camera is started successfully, open another terminal and enter the following command in the terminal to start the hand detection program:

xxxxxxxxxx
ros2 run yahboomcar_mediapipe 01_HandDetector
# Control the robotic arm to look forward
ros2 topic pub /arm6_joints dofbot_pro_interface/msg/ArmJoints "{joint1: 90, joint2: 180, joint3: 0, joint4: 0, joint5: 90, joint6: 0, time: 1000}" --once

After the program runs, as shown in the figure below, the detected hand joint points will be displayed on the right side of the image:

3. Core Code Analysis

Program code path:

• Raspberry Pi 5 mainboard

  The program code is located in the running Docker. The path in the Docker is /root/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_mediapipe/yahboomcar_mediapipe/01_HandDetector.py

• Orin mainboard

  The program code path is /home/jetson/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_mediapipe/yahboomcar_mediapipe/01_HandDetector.py

• RDK mainboard

  The program code path is /home/sunrise/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_mediapipe/yahboomcar_mediapipe/01_HandDetector.py

Import the required library files:

xxxxxxxxxx
import rclpy
from rclpy.node import Node
import mediapipe as mp
import cv2 as cv
import numpy as np
import time
import os
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
from arm_msgs.msg import ArmJoints
import cv2

Initialize the data and define the Publisher and Subscriber:

xxxxxxxxxx
def __init__(self):
    super().__init__('hand_detector')
    self.mpHand = mp.solutions.hands
    self.mpDraw = mp.solutions.drawing_utils
    self.hands = self.mpHand.Hands(
        static_image_mode=False,
        max_num_hands=2,
        min_detection_confidence=0.5,
        min_tracking_confidence=0.5
    )
    self.bridge = CvBridge()
    self.publisher_ = self.create_publisher(PointArray, '/mediapipe/points', 10)
    self.lmDrawSpec = mp.solutions.drawing_utils.DrawingSpec(color=(0, 0, 255), thickness=-1, circle_radius=6)
    self.drawSpec = mp.solutions.drawing_utils.DrawingSpec(color=(0, 255, 0), thickness=2, circle_radius=2)
    # Define the Subscriber for the color image topic
    camera_type = os.getenv('CAMERA_TYPE', 'usb')
    topic_name = '/ascamera_hp60c/camera_publisher/rgb0/image' if camera_type == 'nuwa' else '/usb_cam/image_raw'
    self.subscription = self.create_subscription(
    Image,
    topic_name, 
    self.image_callback,
    10)

Color image callback function:

xxxxxxxxxx
def image_callback(self, msg):
    # Convert the color image message data into image data using CvBridge
    frame = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
    # Pass the obtained image into the defined pubHandsPoint function; draw=False means not drawing the joint points on the original color image
    frame, img = self.pubHandsPoint(frame, draw=True)
    combined = self.frame_combine(frame, img)
    cv.imshow('HandDetector', combined)

The pubHandsPoint function:

xxxxxxxxxx
def pubHandsPoint(self, frame, draw=True):
    # Create a new image based on the size of the input image; the data type of the image is uint8
    img = np.zeros(frame.shape, np.uint8)
    # Convert the color space of the input image from BGR to RGB for subsequent image processing
    img_RGB = cv.cvtColor(frame, cv.COLOR_BGR2RGB)
    # Call the process function in the mediapipe library for image processing; during init, the self.hands object was created and initialized
    self.results = self.hands.process(img_RGB)
    # Determine whether self.results.multi_hand_landmarks exists, that is, whether a palm has been detected
    if self.results.multi_hand_landmarks:
        # Iterate through the palm list to obtain the information of each point
        for i in range(len(self.results.multi_hand_landmarks)):
            if draw: self.mpDraw.draw_landmarks(frame, self.results.multi_hand_landmarks[i], self.mpHand.HAND_CONNECTIONS, self.lmDrawSpec, self.drawSpec)
            # Connect each joint point on the blank image created earlier
            self.mpDraw.draw_landmarks(img, self.results.multi_hand_landmarks[i], self.mpHand.HAND_CONNECTIONS, self.lmDrawSpec, self.drawSpec)
    return frame, img

The frame_combine function for merging images:

def frame_combine(slef,frame, src):
    # Determine whether the image has 3 channels, that is, an RGB image
    if len(frame.shape) == 3:
        # Obtain the sizes of the two images and stitch them together
        frameH, frameW = frame.shape[:2]
        srcH, srcW = src.shape[:2]
        dst = np.zeros((max(frameH, srcH), frameW + srcW, 3), np.uint8)
        dst[:, :frameW] = frame[:, :]
        dst[:, frameW:] = src[:, :]
    else:
        src = cv.cvtColor(src, cv.COLOR_BGR2GRAY)
        frameH, frameW = frame.shape[:2]
        imgH, imgW = src.shape[:2]
        dst = np.zeros((frameH, frameW + imgW), np.uint8)
        dst[:, :frameW] = frame[:, :]
        dst[:, frameW:] = src[:, :]
    return dst