🏷️ AprilTag ID Sorting

1. Course Overview

The function implemented in this lesson can be simply understood as: camera recognizes tags, robotic arm grasps wooden blocks, places them in different positions based on ID.

The specific workflow is as follows:

1. Camera continuously scans the desktop, recognizing AprilTags (black and white square QR codes);

2. Through mathematical calculation, the tag's pixel position is converted to real X, Y, Z 3D coordinates in the robotic arm coordinate frame;

3. After pressing spacebar, the robotic arm automatically solves inverse kinematics to reach the target position and grasp the wooden block;

4. According to the recognized tag ID number (such as 1, 2, 3), the robotic arm rotates to different directions and places the wooden block in the corresponding bin, completing automatic sorting.

2. Preparation and Running

This case requires opening 3 terminals to start the system sequentially:

2.1 Start Camera Driver and Kinematics Solver Environment

Run in Terminal 1 (unified launch of camera driver, robotic arm driver, and inverse solver service):

ros2 launch dofbot_pro_driver A1pro_camera_arm.launch.py

2.2 Start Robotic Arm Desktop Grasping Action Execution Node

Run in Terminal 2:

​x
# Depth camera version (Nuwa)
ros2 run dofbot_pro_driver grasp_desktop

# Or USB camera version
ros2 run dofbot_pro_usb grasp_usb

2.3 Start AprilTag ID Detection and Recognition Master Node

Run in Terminal 3:

xxxxxxxxxx
# Depth camera version (Nuwa)
ros2 run dofbot_pro_driver apriltag_detect

# Or USB camera version
ros2 run dofbot_pro_usb apriltag_usb_detect --ros-args -p tag_size:=0.02

The tag_size:=0.02 parameter for USB camera refers to the real edge length of the AprilTag.

Visualization diagram:

3. Operation Demo and Interaction Flow

1. Place the AprilTag wooden block directly below the robot camera's field of view;

2. The recognition window will draw a green quadrilateral outline, center point, and ID label for the AprilTag;

3. Trigger automatic grasping and sorting:

   • Keep the image window active, press the spacebar (Space);

   • Adaptive distance adjustment and descent: If the wooden block is within the robotic arm's optimal working range, the arm directly descends precisely to grasp; if the distance is slightly off, the chassis will automatically coordinate to fine-tune forward/backward to the optimal grasping point;

   • Multi-ID differentiated unloading: After successful grasping, the robotic arm rotates to specified target positions based on tag ID (e.g., ID:1 places in left bin, ID:2 in right bin, ID:3 in rear bin) to unload, then automatically resets to the initial patrol pose.

4. Core Source Code Analysis

The entire system is completed by two nodes working in collaboration, like the division of labor between a "scout" and an "action team":

Regardless of which camera is used, the overall sorting logic is consistent: Image Acquisition -> Recognize AprilTag -> Calculate 3D Coordinates -> Chassis Fine-tune Alignment -> Robotic Arm IK Descent Grasping -> Rotate and Deliver to Bin Based on ID.

The main technical differences between the two are distance measurement method and spatial compensation dimension:

4.1 USB Monocular Camera Core Source Code Analysis

1. Pinhole Imaging Model Physical Depth Calculation

The USB camera lacks hardware distance measurement capability. The program calculates depth from the pixel size occupied by the tag in the image and the known real edge length (tag_size) using the pinhole imaging formula:

xxxxxxxxxx
# apriltag_usb_detect.py —— Pinhole model depth calculation
def compute_depth_pinhole(self, corners, fx, fy):
    # Calculate average pixel width and height of tag in image
    width_px  = 0.5 * (np.linalg.norm(p1 - p0) + np.linalg.norm(p2 - p3))
    height_px = 0.5 * (np.linalg.norm(p2 - p1) + np.linalg.norm(p3 - p0))

    # Depth = focal length × real size / pixel size
    z_from_w = fx * self.tag_size / width_px
    z_from_h = fy * self.tag_size / height_px
    depth_raw = 0.5 * (z_from_w + z_from_h)

    # Superimpose scale factor depth_scale and intercept depth_bias obtained during calibration
    depth = depth_raw * self.depth_scale + self.depth_bias
    return float(depth)

2. Hand-Eye Transform and Chassis PID Coordination

After calculating 3D coordinates in the camera frame, they are transformed to the base frame through a preset hand-eye matrix. The chassis uses PID controller to fine-tune forward/backward based on measured horizontal distance, keeping the wooden block at the optimal 190mm working zone:

xxxxxxxxxx
# apriltag_usb_detect.py —— Chassis PID coordination fine-tuning
if abs(dist_detect - self.cen_dist) > 5.5:
    # When deviation exceeds 5.5mm, chassis continues to move smoothly
    self.move_dist(dist_detect, cx)
else:
    # Distance is in position, publish target spatial pose to grasping node
    self.pos_info_pub.publish(tag_msg)

3. Inverse Grasping and ID-Based Rotation Sorting

The grasping node calls inverse kinematics (IK) to drive six joints for descent grasping. After successful grasping, differentiated rotation unloading is executed based on tag ID:

xxxxxxxxxx
# grasp_usb.py —— Rotate and deliver to bin based on ID
if self.cur_tagId == 1:
    self.down_joint = [180, 35, 70, 5, 60, self.joint6]  
elif self.cur_tagId == 2:
    self.down_joint = [180, 35, 60, 0, 90, self.joint6]  
elif self.cur_tagId == 3:
    self.down_joint = [0, 30, 70, 2, 90, self.joint6]    
elif self.cur_tagId == 4:
    self.down_joint = [0, 43, 48, 6, 90,self.joint6]   
        
self.pubSixArm(self.down_joint)
time.sleep(2.5)
self.pubSingleArm(6, 30, 2000)  # Open gripper to unload

4.2 3D Depth Camera Core Source Code Analysis

1. RGB-D Soft Timestamp Alignment and Physical Depth Direct Index

The depth camera solution uses ApproximateTimeSynchronizer to ensure strict synchronization between color frames and depth point clouds. After detecting an AprilTag, the true physical distance at the center pixel is directly extracted from the depth map matrix:

xxxxxxxxxx
# apriltag_detect.py —— Direct physical distance index from depth map
cz = depth_image_info[int(center_y), int(center_x)] / 1000.0  # Convert mm to meters

# Quick inverse pinhole transform to camera frame 3D point (Xc, Yc, Zc)
x = (cx - cx_param) * cz / fx
y = (cy - cy_param) * cz / fy
z = cz

2. Adaptive Gripper Rotation Angle Calculation (compute_joint5)

To ensure the gripper can tightly grip the surface regardless of how the wooden block is placed, the depth camera node uses the coordinate difference vector between two adjacent tag corners to calculate the block's deflection angle in real-time and convert it to joint 5 rotation target:

xxxxxxxxxx
# apriltag_detect.py —— Calculate adaptive gripper rotation angle
vx = int(target_tag.corners[0][0]) - int(target_tag.corners[1][0])
vy = int(target_tag.corners[0][1]) - int(target_tag.corners[1][1])
target_joint5 = compute_joint5(vx, vy)

# Publish joint 5 pose angle to grasping node for execution
self.joint5.data = int(target_joint5)
self.TargetJoint5_pub.publish(self.joint5)

3. Hand-Eye 3D Compensation and Full Spatial Computation

After receiving 3D position, the grasping node performs cascaded matrix transformation through hand-eye matrix EndToCamMat and current robotic arm forward kinematics matrix EndPointMat, automatically adding the 3D compensation saved during calibration:

xxxxxxxxxx
# grasp_desktop.py —— Hand-eye transform and 3D physical compensation overlay
PoseEndMat = np.matmul(self.EndToCamMat, self.xyz_euler_to_mat(camera_location, (0, 0, 0)))
EndPointMat = self.get_end_point_mat()
WorldPose = np.matmul(EndPointMat, PoseEndMat)

pose_T, _ = self.mat_to_xyz_euler(WorldPose)

# Superimpose 3D calibration compensation (X forward/backward, Y left/right, Z height)
pose_T[0] = pose_T[0] + self.x_offset
pose_T[1] = pose_T[1] + self.y_offset
pose_T[2] = pose_T[2] + self.z_offset

4. IK Execution and Multi-Bin Automatic Classification

After obtaining the precise spatial pose with compensation, the grasping node asynchronously initiates IK request, drives chassis and robotic arm coordinated descent grasping, and performs multi-station flow placement based on tag ID:

xxxxxxxxxx
# grasp_desktop.py —— IK initiation and bin placement
request = ArmKinemarics.Request()
request.tar_x = pose_T[0]
request.tar_y = pose_T[1]
request.tar_z = pose_T[2]
request.pitch = 1.3962  # Keep gripper vertical downward
request.kin_name = "ik"
future = self.client.call_async(request)

Through this software-hardware layered design, whether you are equipped with a highly cost-effective USB monocular lens or a high-precision 3D depth camera, the system can automatically adapt to the best computation strategy for stable autonomous sorting closed-loop.

4.3 Grasping Offset Tuning (Both Camera Models)

If position deviation occurs during actual grasping (such as missing, knocking over objects, offset left/right, or hitting the table), you can directly modify the spatial offset configuration file for the corresponding camera:

1. Where to Modify

Configuration files are located in the ~/yahboomcar_ros2_ws/dofbot_pro_ws/src/dofbot_pro_driver/config/ directory. Select based on camera type:

• 3D Depth Camera (Nuwa-HP60C): offset_value.yaml

• USB Camera: usb_offset_value.yaml

2. How to Modify

The file contains offset parameters in X, Y, Z directions (unit: meters, recommended adjustment per tweak is 0.005~0.01, i.e., 5~10mm):

• x_offset (forward/backward direction, positive toward vehicle front):

  • Gripper extends too far and knocks over object decrease value (adjust smaller or set negative)

  • Gripper doesn't extend far enough and misses increase value (adjust larger)

• y_offset (left/right direction, positive toward vehicle left):

  • Gripper deviates right increase value (correct left)

  • Gripper deviates left decrease value (correct right)

• z_offset (up/down height, positive toward ceiling):

  • Gripper too high in air and can't grasp decrease value (probe deeper)

  • Gripper hits or scrapes table increase value (raise higher)