Welcome to DOFBOT-Ultra repository
5. MediaPipe Gesture-AprilTag distance sorting
Mediapipe Gesture Recognition AprilTag Distance 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 starting this function, you need to close the web control program backend process
For all functions involving cube block grasping, please use 30x30x30 wooden blocks. Non-standard sized objects may cause servo #6 to stall and suffer damage
1. Function Description
After the program starts, the camera captures images and recognizes gestures. The gestures range from 1 to 5. Based on the recognized gesture, a distance threshold is calculated. The robotic arm will change its posture to detect AprilTags in the image and calculate their height and distance. If there are any with distance less than the threshold, the robotic arm will lower its claw to grasp and place them at the designated location, then return to the AprilTag detection posture to continue recognition. If no AprilTags with distance less than the threshold are detected, the robotic arm will perform a "head shaking" motion sequence and the buzzer will sound, after which the robotic arm returns to the gesture recognition posture.
The distance sorting logic in this tutorial will automatically switch based on
CAMERA_TYPE, but the camera startup command needs to be selected according to the actual camera type:
DaBai depth camera: use
dabai_dcw2.launch.pyRegular USB camera: use
usb_cam.launch.pyBoth modes retain the "sort by distance threshold" functionality:
DaBai depth camera: calculates distance to the robotic arm base using 3D chain;
Regular USB camera: calculates distance of the target on the work plane based on planar calibration results.
2. Startup and Operation
2.1 Startup Commands
x# DaBai depth cameraros2 launch orbbec_camera dabai_dcw2.launch.py
# Regular USB cameraros2 launch dofbot_ultra_info usb_cam.launch.py
# Start underlying control:ros2 run dofbot_ultra_driver arm_driver# Start inverse kinematics program:ros2 run dofbot_ultra_info kinematics_dofbotultra# Start image conversion program:ros2 run dofbot_ultra_apriltag msgToimg# Start AprilTag recognition program:ros2 run dofbot_ultra_apriltag apriltag_list_Dist# Start robotic arm grasping program:ros2 run dofbot_ultra_driver grasp # Start Mediapipe gesture recognition program:ros2 run dofbot_ultra_apriltag MediapipeGesture# Depth camera exposure settingsros2 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:
The camera startup command needs to be selected according to the actual camera type.
Except for the camera startup command, the startup methods for other nodes remain the same.
The blue area in the image represents the wooden blocks that will be grasped (distance less than threshold), while blocks outside the blue area will not be grasped
2.2 Operation
After the program starts, the robotic arm will initially present the gesture recognition posture. Recognizable gestures range from 1 to 5. Gesture recognition waits for about 3 seconds for the AprilTag to change posture, then switches to the AprilTag detection and recognition posture. Press the spacebar to start recognition. If an AprilTag with distance less than the calculated distance threshold is recognized, the robotic arm will lower its claw to grasp that AprilTag wooden block and place it at the designated location. After placement is complete, the robotic arm returns to the AprilTag recognition posture, and the next recognition requires pressing the spacebar again. If no AprilTag with distance less than the set threshold is recognized, the robotic arm will perform a "head shaking" motion sequence, after which the robotic arm returns to the gesture recognition posture.
The distance calculation method will automatically switch based on the current CAMERA_TYPE:
Depth camera mode: calculates the distance from the target to the robotic arm base using 3D chain;
Regular USB camera mode: calculates the distance of the target on the work plane based on planar calibration results.
3. Program Flowchart
apriltag_list_Dist.py
4. Core Code Analysis
4.1 MediapipeGesture.py
Code path:
xxxxxxxxxxRaspberry Pi:/home/yahboom/dofbot_ultra_ws/src/dofbot_ultra_apriltag/dofbot_ultra_apriltag/MediapipeGesture.pyRDK:/home/sunrise/dofbot_ultra_ws/src/3d/dofbot_ultra_apriltag/dofbot_ultra_apriltag/MediapipeGesture.py
Please refer to section 4.1 【MediapipeGesture.py】 in the tutorial 【3D Sorting and Grasping Course - 3. MediaPipe Gesture-AprilTag ID sorting】.
4.2 apriltag_list_Dist.py
Code path:
xxxxxxxxxxRaspberry Pi:/home/yahboom/dofbot_ultra_ws/src/dofbot_ultra_apriltag/dofbot_ultra_apriltag/apriltag_list_Dist.pyRDK:/home/sunrise/dofbot_ultra_ws/src/3d/dofbot_ultra_apriltag/dofbot_ultra_apriltag/apriltag_list_Dist.py
Import necessary libraries,
xxxxxxxxxximport cv2import rclpyfrom rclpy.node import Nodeimport numpy as npfrom message_filters import ApproximateTimeSynchronizer, Subscriberfrom sensor_msgs.msg import Imagefrom std_msgs.msg import Float32, Int8, Boolfrom dt_apriltags import Detectorfrom dofbot_ultra_apriltag.vutils import draw_tagsfrom cv_bridge import CvBridgeimport cv2 as cvfrom dofbot_ultra_interface.srv import * from dofbot_ultra_interface.msg import ArmJoint, AprilTagInfoimport pyzbar.pyzbar as pyzbarimport timeimport queueimport mathimport osencoding = ['16UC1', '32FC1']import threading# Import transforms3d library for handling transformations in 3D space, performing conversions between quaternions, rotation matrices, and Euler angles, supporting 3Dimport transforms3d as tfs# Import transformations to handle and calculate transformations in 3D space, including conversions between quaternions and Euler anglesimport tf_transformations as tf from Arm_Lib import Arm_Device
Program parameter initialization, creating publishers and subscribers,
xxxxxxxxxxdef __init__(self): rospy.init_node('apriltag_detect') super().__init__('apriltag_detect')
# 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] self.Arm = Arm_Device() # 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 (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 self.done_flag = True
self.set_height = 40.0 self.set_dist = 0.0 self.detect_flag = False # AprilTag detector (configuration remains 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 ) self.target_id = 0 self.pr_time = time.time() self.cnt = 0 self.Center_x_list = [] self.Center_y_list = [] self.search_joints = [90.0,150.0,12.0,20.0,90.0,30.0]
self.CurEndPos = [-0.006,0.116261662208,0.0911289015753,-1.04719,-0.0,0.0] self.camera_info_K = [477.57421875, 0.0, 319.3820495605469, 0.0, 477.55718994140625, 238.64108276367188, 0.0, 0.0, 1.0] self.EndToCamMat = np.array([[1.00000000e+00,0.00000000e+00,0.00000000e+00,0.00000000e+00], [0.00000000e+00,7.96326711e-04,9.99999683e-01,-9.90000000e-02], [0.00000000e+00,-9.99999683e-01,7.96326711e-04,4.90000000e-02], [0.00000000e+00,0.00000000e+00,0.00000000e+00,1.00000000e+00]])Main focus on the image processing function TagDetect,
xxxxxxxxxxdef TagDetect(self,color_frame,depth_frame): #rgb_image #Receive color image topic message, convert message data to image data rgb_image = self.rgb_bridge.imgmsg_to_cv2(color_frame,'rgb8') result_image = np.copy(rgb_image) #depth_image #Receive depth image topic message, convert message data 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 detect function, pass parameters, ''' cv2.cvtColor(rgb_image, cv2.COLOR_RGB2GRAY): Convert RGB image to grayscale image for tag detection. False: Indicates not to estimate tag pose. None: Indicates camera parameters not provided, may only perform simple detection. 0.025: May be the set tag size (unit usually meters), used to help detection algorithm determine tag size Returns a detection result, including position, ID, and bounding box for each tag. ''' tags = self.at_detector.detect(cv2.cvtColor(rgb_image, cv2.COLOR_RGB2GRAY), False, None, 0.025) #Sort tags in the tags list, not a required step tags = sorted(tags, key=lambda tag: tag.tag_id) # Seems to come out in ascending order, no need for manual sorting #Call draw_tags function, which draws information about recognized AprilTags on the color image, including corners, center points, and id values draw_tags(result_image, tags, corners_color=(0, 0, 255), center_color=(0, 255, 0)) key = cv.waitKey(10) #Define the length of self.Center_x_list and self.Center_y_list self.Center_x_list = list(range(len(tags))) self.Center_y_list = list(range(len(tags))) #Wait for keyboard input, 32 means spacebar pressed, after pressing change self.pubPos_flag value, indicating AprilTag information can be published if key == 32: self.pubPos_flag = True #Check tags length, greater than 0 means AprilTag detected and grasp completion flag if len(tags) > 0 and self.done_flag == True: #Iterate through AprilTags for i in range(len(tags)): #AprilTag center xy values stored in Center_x_list and Center_y_list center_x, center_y = tags[i].center self.Center_x_list[i] = center_x self.Center_y_list[i] = center_y cx = center_x cy = center_y #Calculate depth value of center coordinates cz = depth_image_info[int(cy),int(cx)]/1000 #Call compute_heigh function to calculate AprilTag height, parameters are AprilTag center coordinates and center point depth value, returns a position list, pose[2] is z value, i.e., height value pose = self.compute_heigh(cx,cy,cz) heigh_detect = round(pose[2],4)*1000 - 12 heigh = 'heigh: ' + str(heigh_detect) + 'mm' #Calculate distance value of AprilTag from base coordinate system, here uses Euclidean distance formula to calculate distance of xy from origin (0,0), scale the value and convert unit to mm, pose[1] is y value of center point in world coordinate system, pose[0] is x value of center point in world coordinate system dist_detect = math.sqrt(pose[1] ** 2 + pose[0]** 2) dist_detect = round(dist_detect,3)*1000 dist = 'dist: ' + str(dist_detect) + 'mm' #Height and distance values are drawn on color image using opencv cv.putText(result_image, heigh, (int(cx)+5, int(cy)-15), cv.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 2) cv.putText(result_image, dist, (int(cx)+5, int(cy)+15), cv.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 2) #If detected AprilTag distance from base coordinate system is less than set distance threshold and self.pubPos_flag is True and set distance threshold is not 0 if dist_detect<=self.set_dist and self.set_dist!=0 and self.pubPos_flag == True: print("self.set_dist: ",self.set_dist) print("dist_detect: ",dist_detect) #Change self.detect_flag value, True means recognized AprilTag with distance higher than set threshold self.detect_flag = True tag = AprilTagInfo() tag.id = tags[i].tag_id tag.x = self.Center_x_list[i] tag.y = self.Center_y_list[i] tag.z = depth_image_info[int(tag.y),int(tag.x)]/1000 #If depth information is not 0, indicates valid data, then publish AprilTag information message if tag.z!=0 : self.tag_info_pub.publish(tag) self.pubPos_flag = False self.done_flag = False else: print("Invalid distance.") #If self.detect_flag is False means no AprilTag with distance less than threshold recognized and set distance threshold is not 0 and AprilTag message publishing enabled, meeting all three conditions means no AprilTag with distance less than threshold recognized. if self.detect_flag != True and self.set_dist!=0 and self.pubPos_flag==True: print("----------------------------------------") self.set_dist!=0 self.shake() #time.sleep(2) self.pub_arm(self.search_joints) grasp_done = Bool() grasp_done.data = True #Publish grasp completion topic, so that next gesture recognition node program can publish gesture recognition results self.pubGraspStatus.publish(grasp_done) #time.sleep(2) self.pubPos_flag = False #If no AprilTag recognized after pressing spacebar, robotic arm also performs "head shaking" motion sequence, then returns to gesture recognition posture elif self.pubPos_flag == True and len(tags) == 0: self.shake() self.pub_arm(self.search_joints) grasp_done = Bool() grasp_done.data = True self.pubGraspStatus.publish(grasp_done) result_image = cv.cvtColor(result_image, cv.COLOR_RGB2BGR) cv.imshow("result_image", result_image) key = cv.waitKey(1)Callback function for gesture recognition results GetTargetIDCallback,
xxxxxxxxxxdef GetTargetIDCallback(self,msg): print("msg.data: ",msg.data) #Calculate distance threshold unit is mm, minimum is 160mm, maximum is 200mm self.set_dist = 150 + msg.data*10 #Calculate height threshold unit is mm, minimum is 40mm, maximum is 80mm self.set_height = 30 + msg.data*10 print("self.set_height: ",self.set_height) #After receiving message, change robotic arm posture to present AprilTag recognition posture self.pub_arm(self.init_joints)4.3 grasp.py
Please refer to section 4.2 【grasp.py】 in the tutorial 【3D Sorting and Grasping Course - 1. AprilTag ID sorting】.