Welcome to DOFBOT-Ultra repository

MediaPipe Gesture – Machine Code ID Sorting

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

Before enabling this feature, the web control program backend process must be shut down.

For all cube picking functions, please use 30x30x30 mm wooden blocks. Non-standard sizes may cause servo #6 to stall and become damaged.

1. Function Description

Once the program starts, the camera captures images and recognizes hand gestures (1 to 4). Based on the recognized gesture, a target machine code ID is assigned. The robotic arm then changes its posture to search for the AprilTag with that ID. If the target tag is detected, the arm picks it up and places it at a preset location, then returns to the detection posture to continue recognition. If the target tag is not found, the arm returns to the gesture recognition posture.

The vision and grasping logic in this tutorial switches automatically based on CAMERA_TYPE, but the camera launch command must be selected according to the actual camera type:

• DaBai Depth Camera: use dabai_dcw2.launch.py

• Standard USB Camera: use usb_cam.launch.py

2. Startup and Operation

2.1. Startup Commands

Enter the following commands in the terminal:

​x
# DaBai Depth Camera
ros2 launch orbbec_camera dabai_dcw2.launch.py

# Standard USB Camera
ros2 launch dofbot_ultra_info usb_cam.launch.py

# Start low-level control:
ros2 run dofbot_ultra_driver arm_driver
# Start inverse kinematics:
ros2 run dofbot_ultra_info kinematics_dofbotultra
# Start image conversion:
ros2 run dofbot_ultra_apriltag msgToimg
# Start tag recognition:
ros2 run dofbot_ultra_apriltag apriltagID_finger_detect
# Start grasping program:
ros2 run dofbot_ultra_driver grasp 
# Start MediaPipe gesture recognition:
ros2 run dofbot_ultra_apriltag MediapipeGesture

# Set exposure
ros2 service call /camera/set_color_exposure orbbec_camera_msgs/srv/SetInt32 '{data: 30}'
ros2 service call /camera/set_depth_auto_exposure std_srvs/srv/SetBool '{data: true}'

Notes:

1. The camera launch command should be chosen based on the actual camera type.

2. The other nodes are started in the same way regardless of camera type.

2.2. Operation

After the program starts, the robotic arm initially assumes a gesture recognition posture. The recognizable gestures are 1 through 4, as shown below. Gesture recognition takes about 5 seconds (watch the console output; once the counter reaches 14, the gesture is confirmed). The arm then changes to the tag detection posture.

If a target tag is detected, the robotic arm automatically selects the grasp coordinate calculation method based on the current CAMERA_TYPE:

• Depth camera mode: calculates the grasp point using the center-point depth value;

• Standard USB camera mode: calculates the grasp point using planar calibration results.

The arm then grasps and places the object at the designated location. If no tag matching the target ID is found after a period of time, the arm returns to the initial gesture recognition posture.

Note: The illustration above shows the case when gesture 2 is recognized.

3. Program Flowchart

MediapipeGesture.py

apriltagID_finger_detect.py

4. Core Code Analysis

4.1. MediapipeGesture.py

Code path:

xxxxxxxxxx
Raspberry Pi: /home/yahboom/dofbot_ultra_ws/src/dofbot_ultra_apriltag/dofbot_ultra_apriltag/MediapipeGesture.py
RDK: /home/sunrise/dofbot_ultra_ws/src/3d/dofbot_ultra_apriltag/dofbot_ultra_apriltag/MediapipeGesture.py

Import required libraries:

xxxxxxxxxx
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile
import numpy as np
from dofbot_ultra_apriltag.media_library import *
from collections import defaultdict
import threading
from std_msgs.msg import Float32, Int8, Bool
from dofbot_ultra_interface.msg import *
from time import sleep, time

Program parameter initialization, creating publishers and subscribers:

xxxxxxxxxx
def __init__(self):
    super().__init__('detect_gesture_node')
   self.hand_detector = HandDetector()
        self.pub_gesture = True
        
        # ROS2 Subscribers
        qos = QoSProfile(depth=10)
        self.img_sub = self.create_subscription(ImageMsg, "/image_data",self.image_sub_callback, qos)
        self.grasp_sub= self.create_subscription(Bool,'grasp_done',self.GraspStatusCallback, qos)
        # ROS2 Publishers
        self.pubPoint = self.create_publisher(ArmJoint, "TargetAngle", qos)
        self.pub_targetID = self.create_publisher(Int8, "TargetId", qos)
        self.pTime = self.cTime = 0
        self.cnt = 0
        self.last_sum = 0
        self.detect_gesture = Int8()
        self.pTime = 0
        # Initialize other variables
        self.detect_gesture_joints = [90.0, 150.0, 12.0, 20.0, 90.0, 30.0]
        self.img = np.zeros((480, 640, 3), dtype=np.uint8)
        self.pubTargetArm(self.detect_gesture_joints)

Image processing function image_sub_callback:

xxxxxxxxxx
def image_sub_callback(self,msg):
    # Convert custom image message to image array
    image = np.ndarray(shape=(msg.height, msg.width, msg.channels), dtype=np.uint8, buffer=msg.data)
    # Convert RGB to OpenCV BGR order
    self.img[:,:,0],self.img[:,:,1],self.img[:,:,2] = image[:,:,2],image[:,:,1],image[:,:,0] 
    frame = self.img.copy()
    # Pass the converted image to the process function for recognition
    self.process(frame)

Image processing function process:

xxxxxxxxxx
def process(self, frame):
    # Pass the image to the hand_detector's findHands function for real-time hand tracking and detection
    frame, lmList, bbox = self.hand_detector.findHands(frame)
    # If detection result is not empty, a hand has been detected; pass results to Gesture_Detect_threading and start the thread
    if len(lmList) != 0:
        threading.Thread(target=self.Gesture_Detect_threading, args=(lmList,bbox)).start()
    # Calculate FPS
    self.cTime = time()
    fps = 1 / (self.cTime - self.pTime)
    self.pTime = self.cTime
    text = "FPS : " + str(int(fps))
    # Draw FPS on the color image
    cv.putText(frame, text, (20, 30), cv.FONT_HERSHEY_SIMPLEX, 0.9, (0, 0, 255), 1)
    if cv.waitKey(1) & 0xFF == ord('q'):
        cv.destroyAllWindows()
        rospy.signal_shutdown("exit")
    cv.imshow('frame', frame)

Gesture recognition thread function Gesture_Detect_threading:

xxxxxxxxxx
def Gesture_Detect_threading(self, lmList,bbox):
    # Pass the hand detection result to the fingersUp function, which returns a list of extended fingers.
    # The list index corresponds to the 5 fingers; if a finger is extended, its value is 1, otherwise 0.
    fingers = self.hand_detector.fingersUp(lmList)
    # Calculate the sum of the fingers list to determine how many fingers are extended.
    # Assign this value to self.last_sum and compare with the previous count.
    self.last_sum = sum(fingers)
    print(self.pub_gesture)
    if sum(fingers) == self.last_sum:
        print("---------------------------")
        self.cnt = self.cnt + 1
        print("cnt: ",self.cnt)
        # If the current finger count equals the previous one, increment the counter.
        # When the count reaches 30 and self.pub_gesture is True, publish the /TargetId topic.
        if self.cnt==30 and self.pub_gesture == True:
            print("sum of fingers: ",self.last_sum)
            # Change self.pub_gesture status to prevent false re-publication
            self.pub_gesture = False
            # Assign the value to the message data; self.last_sum corresponds to the target machine code ID
            self.detect_gesture.data = self.last_sum   
            self.pub_targetID.publish(self.detect_gesture)
            # Reset data for next recognition cycle
            self.last_sum = 0

4.2. apriltagID_finger_detect.py

Code path:

xxxxxxxxxx
Raspberry Pi: /home/yahboom/dofbot_ultra_ws/src/dofbot_ultra_apriltag/dofbot_ultra_apriltag/apriltagID_finger_detect.py
RDK: /home/sunrise/dofbot_ultra_ws/src/3d/dofbot_ultra_apriltag/dofbot_ultra_apriltag/apriltagID_finger_detect.py

Import required libraries:

xxxxxxxxxx
import cv2
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile
import numpy as np
from message_filters import ApproximateTimeSynchronizer, Subscriber
from sensor_msgs.msg import Image
from std_msgs.msg import Float32, Int8, Bool
from dt_apriltags import Detector
from dofbot_ultra_apriltag.vutils import draw_tags
from cv_bridge import CvBridge
import cv2 as cv
from dofbot_ultra_interface.srv import *  # Assuming services have been migrated
from dofbot_ultra_interface.msg import ArmJoint, AprilTagInfo
import pyzbar.pyzbar as pyzbar
import time
import queue
import os
encoding = ['16UC1', '32FC1']

Program parameter initialization, creating publishers and subscribers:

xxxxxxxxxx
def __init__(self):
    super().__init__('apriltag_detect_node')  # ROS2 node initialization
        
        # Initialize parameters
        self.detect_joints = [90.0, 150.0, 12.0, 20.0, 90.0, 30.0]
        self.init_joints = [90.0, 120.0, 0.0, 0.0, 90.0, 90.0]
        self.search_joints = [90.0, 120.0, 0.0, 0.0, 90.0, 30.0]
    
        
        # ROS2 Publishers
        self.pubGraspStatus = self.create_publisher(Bool, "grasp_done", 1)
        self.tag_info_pub = self.create_publisher(AprilTagInfo, "PosInfo", 1)
        self.pubPoint = self.create_publisher(ArmJoint, "TargetAngle", 1)
        
        # ROS2 Subscribers (with message synchronization)
        self.depth_image_sub  = Subscriber(self, Image, "/camera/color/image_raw", qos_profile=1)
        self.rgb_image_sub = Subscriber(self, Image, "/camera/depth/image_raw", qos_profile=1)
        self.TimeSynchronizer = ApproximateTimeSynchronizer([self.depth_image_sub, self.rgb_image_sub],queue_size=10,slop=0.5)
        self.TimeSynchronizer.registerCallback(self.TagDetect)

        # ROS2 Other Subscribers
        self.grasp_status_sub = self.create_subscription(Bool, 'grasp_done', self.GraspStatusCallback, 1)
        self.sub_targetID = self.create_subscription(Int8, "TargetId", self.GetTargetIDCallback, 1)
        
        # Initialize tools
        self.rgb_bridge = CvBridge()
        self.depth_bridge = CvBridge()
        self.pubPos_flag = False
        
        # AprilTag detector (configuration unchanged)
        self.at_detector = Detector(
            searchpath=['apriltags'],
            families='tag36h11',
            nthreads=8,
            quad_decimate=2.0,
            quad_sigma=0.0,
            refine_edges=1,
            decode_sharpening=0.25,
            debug=0
        )
        
        # State variables
        self.pr_time = time.time()
        self.target_id = 0
        self.cnt = 0
        self.detect_flag = False
        self.pub_arm(self.detect_joints)

Focus on the image processing function TagDetect:

xxxxxxxxxx
def TagDetect(self,color_frame,depth_frame):
    #rgb_image
    #Receive the color image topic message and convert it to image data
    rgb_image = self.rgb_bridge.imgmsg_to_cv2(color_frame,'rgb8')
    result_image = np.copy(rgb_image)
    #depth_image
    #Receive the depth image topic message and convert it to image data
    depth_image = self.depth_bridge.imgmsg_to_cv2(depth_frame, encoding[1])
    frame = cv.resize(depth_image, (640, 480))
    depth_image_info = frame.astype(np.float32)
    #Call the detect function with parameters
    '''
    cv2.cvtColor(rgb_image, cv2.COLOR_RGB2GRAY): converts the RGB image to grayscale for tag detection.
    False: do not estimate tag pose.
    None: no camera parameters provided, simple detection only.
    0.025: estimated tag size (in meters), used to help the detection algorithm determine tag scale.
    Returns detection results including each tag's position, ID, and bounding box.
    '''
    tags = self.at_detector.detect(cv2.cvtColor(rgb_image, cv2.COLOR_RGB2GRAY), False, None, 0.025)
    #Sort the tags in the list (non-essential)
    tags = sorted(tags, key=lambda tag: tag.tag_id) # appears to be ascending by default, manual sorting not required
    #Call draw_tags to annotate the color image with tag information (corners, center, ID)
    draw_tags(result_image, tags, corners_color=(0, 0, 255), center_color=(0, 255, 0))
    #Wait for keyboard input; 32 is the space key. Pressing space toggles self.pubPos_flag to allow publishing tag info
    key = cv2.waitKey(10)
    if key == 32:
        self.pubPos_flag = True
    if self.target_id!=0:
        print("Get th target id,start to search it.")
        self.pub_arm(self.search_joints)
        #time.sleep(0.5)
        #If tags list is not empty, machine codes have been detected
        if len(tags) > 0 :
            #Iterate through detected tags
            for i in range(len(tags)):
                #self.tag_info_pub.publish(tag)
                if self.pubPos_flag == True:
                    center_x, center_y = tags[i].center
                    cv.circle(result_image, (int(center_x),int(center_y)), 10, (0,210,255), thickness=-1)
                    print("tag_id: ",tags[i].tag_id)
                    print("center_x, center_y: ",center_x, center_y)
                    print("depth: ",depth_image_info[int(center_y),int(center_x)]/1000)
                    #Create tag info message
                    tag = AprilTagInfo()
                    tag.id = tags[i].tag_id
                    tag.x = center_x
                    tag.y = center_y
                    tag.z = depth_image_info[int(center_y),int(center_x)]/1000
                    #If the center depth of the current tag is > 0 and the tag ID matches the target ID, publish the topic
                    if tag.z>0 and tag.id == self.target_id:
                        self.tag_info_pub.publish(tag)
                        self.pubPos_flag = False
                        self.cnt = 0
                    else:
                        if tag.z!=0:
                            print("Invalid distance.")
                        if tag.id != self.target_id:
                            print("Do not find the target id tag.")
                            self.cnt = self.cnt + 1
                            #If not the target ID, increment counter. When counter reaches 20, no matching ID was found; return to gesture detection posture
                            if self.cnt == 20:
                                self.cnt = 0
                                #Return to gesture detection posture
                                self.pub_arm(self.detect_joints)
                                self.target_id = 0
        else:
            self.cnt = self.cnt + 1
            #If no machine code is detected at all, increment counter. When it reaches 100, the arm returns to gesture detection posture
            if self.cnt == 100:
                self.cnt = 0
                self.pub_arm(self.detect_joints)
                self.target_id = 0
    result_image = cv2.cvtColor(result_image, cv2.COLOR_RGB2BGR)
    cv2.imshow("result_image", result_image)
    key = cv2.waitKey(1)

Two callback functions:

xxxxxxxxxx
#Grasp status callback
def GraspStatusCallback(self,msg):
    #Toggle self.pubPos_flag to allow publishing the topic again
    if msg.data == True:
        self.pubPos_flag = True
#Target ID callback        
def GetTargetIDCallback(self,msg):
    #Update self.target_id
    self.target_id = msg.data
    print("Get th traget is ",self.target_id)

4.3. grasp.py

Please refer to section 4.2 【grasp.py】 in the tutorial 【3D Sorting and Grasping Course - 1. AprilTag ID sorting】