Welcome to DOFBOT-Pi repository

Mediapipe Gesture-AprilTag Distance Sorting

1. Function Description

After the program runs, the robotic arm will lift up, then recognize gestures. The gestures are 1-5 (corresponding to the number of extended fingers), and then the robotic arm will move to the recognition posture. It will recognize AprilTags on the desktop and calculate the distance between the AprilTags and the robotic arm base coordinate system. If there exists an AprilTag whose distance from the robotic arm base coordinate system is greater than the sorting distance, the robotic arm will lower its gripper to grasp that AprilTag wooden block; otherwise, it will lift the robotic arm for the next gesture recognition. The sorting distance calculation formula is gesture x 0.01 x 2 + 0.14.

2. Function Startup

After entering the docker container, enter the following commands in the docker terminal:

# Start USB camera node and inverse kinematics node
ros2 launch dofbot_info camera_arm_kin.launch.py
# Start gesture recognition node
ros2 run dofbot_sorting mediapipe_gesture
# Start AprilTag ID recognition node
ros2 run dofbot_sorting apriltag_gesture_dist
# Start grasping node
ros2 run dofbot_sorting grasp

After successfully running the program, the robotic arm will move to its initial position, then lift up and enter the gesture recognition posture. At this time, we can give gestures 1-5 by extending the corresponding number of fingers. After the program accumulates 40 recognitions and the gesture has not changed, it will publish the gesture recognition result and the robotic arm will move to the AprilTag recognition posture to start recognizing AprilTags. As shown in the figure below, place the AprilTag wooden blocks within the robotic arm's camera view.

Then press the spacebar to start recognition:

• If there exists an AprilTag whose distance from the base coordinate is greater than the sorting distance, the robotic arm will lower its gripper to grasp that AprilTag wooden block;
• If there is no AprilTag greater than the sorting distance or no AprilTag wooden blocks are recognized, the robotic arm will move to the gesture recognition posture for the next gesture recognition.

3. Code Analysis

3.1. mediapipe_gesture

Already analyzed in [3D Space Sorting and Grasping][2. Mediapipe Gesture Recognition AprilTag ID Sorting]-[3. Code Analysis], you can refer to the content there.

3.2. apriltag_gesture_dist

After entering the docker container, the source code path is located at:

xxxxxxxxxx
/root/dofbot_sorting/dofbot_sorting/apriltag_gesture_dist.py

Import necessary library files:

xxxxxxxxxx
import rclpy
from rclpy.node import Node
import cv2
import numpy as np
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2 as cv
from dt_apriltags import Detector
from dofbot_sorting.vutils import draw_tags
from dofbot_interface.srv import Kinemarics
from dofbot_interface.msg import *
from std_msgs.msg import Bool,Int16
import time
from Arm_Lib import Arm_Device
import os
from dofbot_sorting.compute_joint5 import *

Create robotic arm control object and control the robotic arm to move to initial position:

xxxxxxxxxx
self.Arm = Arm_Device()
self.init_joints = [90.0, 120.0, 0.0, 0.0, 90.0, 0.0]
self.Arm.Arm_serial_servo_write6_array(self.init_joints,2000)

Create publishers and subscribers:

xxxxxxxxxx
# Create publisher to publish recognized position information topic messages
self.pos_info_pub = self.create_publisher(AprilTagInfo, "PosInfo",
qos_profile=10)
# Create subscriber to subscribe to grasp completion topic messages
self.sub_grasp_done =
self.create_subscription(Bool,'grasp_done',self.GraspStatusCallback,qos_profile=
1)3
# Create subscriber to subscribe to gesture recognition result topic
self.sub_gestureID =
self.create_subscription(Int16,"GesturetId",self.GestureIDCallback,1)
# Create publisher to publish reset gesture topic messages
self.pub_reset = self.create_publisher(Bool, "/reset_gesture", qos_profile=10)
# Create publisher to publish calculated joint 5 angle reference value topic messages
self.pub_joint5 = self.create_publisher(Int16, "set_joint5", 10)

Grasp completion topic callback function GraspStatusCallback:

xxxxxxxxxx
def GraspStatusCallback(self,msg):
    print("**")
    if msg.data == True:
        # Assign self.pubPos_flag value to True, indicating that position information topic can be published
        self.pubPos_flag = True
        self.detect_flag = False

Gesture recognition result topic callback function:

xxxxxxxxxx
def GestureIDCallback(self,msg):
    # Calculate sorting distance
    self.target_dist = msg.data*0.01*2 + self.target_dist
    print("self.target_dist: ",self.target_dist)
    time.sleep(3.0)
    self.pubPos_flag = True

Image topic callback function ImageCallback:

​x
def ImageCallback(self,color_frame):
    # Process received image topic messages, convert to image
    rgb_image = self.rgb_bridge.imgmsg_to_cv2(color_frame,'rgb8')
    result_image = np.copy(rgb_image)
    tags = self.at_detector.detect(cv2.cvtColor(rgb_image, cv2.COLOR_RGB2GRAY), False, None, 0.025)
    tags = sorted(tags, key=lambda tag: tag.tag_id) # Seems to be already in ascending order, no need for manual sorting
    draw_tags(result_image, tags, corners_color=(0, 0, 255), center_color=(0, 255, 0))
    key = cv2.waitKey(10)
    if key == 32:
        self.pubPos_flag = True
    if len(tags) > 0 :
        for i in range(len(tags)):
            #self.detect_flag = True
            #self.pubPos_flag = True
            if self.pubPos_flag == True:
                # Extract AprilTag center point coordinates
                center_x, center_y = tags[i].center
                cx = center_x
                cy = center_y
                cz = self.dist
                # Based on AprilTag center point coordinates, convert coordinates to normalized coefficients a and b. a is x-axis offset coefficient, b is y-axis offset coefficient. The calculated ba values are the center point coordinates in the world coordinate system, which will eventually be sent to the sorting
​
                (a, b) = (round(((320 - cx) / 4000), 5), round(((480 - cy) / 3000) * 0.8+0.15, 5))
                print("a: ",a)
                print("b: ",b) 
                # Calculate joint 5 angle reference value based on corner offset angle
                vx = int(tags[i].corners[0][0]) - int(tags[i].corners[1][0])
                vy = int(tags[i].corners[0][1]) - int(tags[i].corners[1][1])
                target_joint5 = compute_joint5(vx,vy)
                print("target_joint5: ",target_joint5)
                # If AprilTag distance is greater than sorting distance, i.e., outside the sorting distance
                if self.pubPos_flag == True and  b > self.target_dist:
                    self.detect_flag = True
                    # Assign self.pubPos_flag value to False to prevent multiple repeated publications of position information topic
                    self.pubPos_flag = False
                    joint5 = Int16()
                    joint5.data = int(target_joint5)
                    self.pub_joint5.publish(joint5)
                    # Create position information topic message object
                    pos = AprilTagInfo()
                    pos.x = b
                    pos.y = a
                    pos.z = 0.03
                    pos.id = tags[i].tag_id
                    # Publish position information topic
                    self.pos_info_pub.publish(pos)
        # If no AprilTag found outside sorting distance, publish reset gesture topic message
        if self.detect_flag == False and self.pubPos_flag == True:
            reset = Bool()
            reset.data = True
            self.pub_reset.publish(reset)
            self.pubPos_flag = False
            self.target_dist = 0.14
            print("Do not find the target dist tag.")
    else:
        # If spacebar was pressed and no AprilTag was found, publish reset gesture topic message
        if self.pubPos_flag == True:
            print("Do not find any tags")
            reset = Bool()
            reset.data = True
            self.pub_reset.publish(reset)
            self.pubPos_flag = False

3.3. grasp

Already analyzed in [13.3D Sorting and Grasping Course]-[1. AprilTag ID Sorting]-[3. Code Analysis], you can refer to the content there.

4. Node Communication Diagram

After entering the docker container, enter the following commands in the docker terminal:

xxxxxxxxxx
ros2 run rqt_graph rqt_graph