Welcome to DOFBOT-Ultra repository

AprilTag Tracking and Grasping

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

The web control program backend process must be closed before starting this function.

For all functions involving grasping cube-shaped objects, please use 30x30x30 mm wooden blocks. Non-standard sized objects may cause servo 6 to stall and overheat.

1. Function Description

After the program starts, it will recognize the AprilTag and then track it so that the center of the AprilTag aligns with the center of the image. In DaBai depth camera mode, once the robotic arm stabilizes, it will wait for the target to stabilize, then beep when the depth distance is valid, grasp the AprilTag, place it at the set position, and return to the recognition pose. In standard USB camera mode, only tracking is performed; beeping, grasping, and placement actions are not triggered.

The tracking and grasping logic in this tutorial automatically switches 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

The behavior differs between the two modes:

• DaBai depth camera: tracks via depth pipeline and grasps;

• Standard USB camera: tracks via RGB-only, performs target following without grasping.

2. Launch and Operation

2.1. Launch Commands

Enter the following commands in the terminal to launch:

​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 AprilTag tracking and grasping:
ros2 run dofbot_ultra_apriltag apriltag_follow

# Set depth camera 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 must be selected according to the actual camera type.

2. Apart from the camera launch command, all other nodes are started in the same way.

2.2. Operation

After the program starts, hold a 3cm x 3cm AprilTag block in the camera view. The robotic arm will adjust its pose so that the center of the AprilTag aligns with the center of the image. Slowly move the AprilTag, and the robotic arm will follow it, continuously adjusting its pose. Once the AprilTag center aligns with the image center, the program will automatically select the operating mode based on the current CAMERA_TYPE:

• Depth camera mode: grasps when the center point depth value is valid and within the appropriate working distance;

• Standard USB camera mode: continuously tracks via RGB-only; no grasping is performed even when the target is centered and stable.

After grasping in depth camera mode, the arm places the object at the set position and finally returns to the recognition pose. USB mode maintains the tracking flow.

3. Program Flowchart

4. Core Code Analysis

This tutorial actually launches:

xxxxxxxxxx
ros2 run dofbot_ultra_apriltag apriltag_follow

The actual source code being executed is:

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

4.1. Detection and Tracking Main Node apriltag_follow.py

The current version of apriltag_follow.py implements dual-camera support:

1. When CAMERA_TYPE=DABAI_DCW2, it subscribes to both color and depth images, computes distance via the depth camera pipeline, and allows grasping;

2. In standard USB camera mode, it only subscribes to RGB images, tracks via RGB-only, and no longer triggers grasping.

xxxxxxxxxx
if USE_DEPTH_CAMERA:
    self.rgb_image_sub = Subscriber(self, Image, "/camera/color/image_raw", qos_profile=1)
    self.depth_image_sub = Subscriber(self, Image, "/camera/depth/image_raw", qos_profile=1)
    self.TimeSynchronizer = ApproximateTimeSynchronizer(
        [self.rgb_image_sub, self.depth_image_sub], queue_size=10, slop=0.5)
    self.TimeSynchronizer.registerCallback(self.DetectAndRemove)
else:
    self.rgb_image_sub = self.create_subscription(
        Image, "/camera/color/image_raw", self._on_usb_image, make_usb_image_qos())

In DetectAndRemove(), the node performs the following tasks:

1. Detects the AprilTag and obtains the target center point;

2. If the target deviates from the image center, calls DofbotTrack.XY_track() for XY following;

3. Only if the target remains continuously stable in the center area does it further evaluate whether grasping is allowed.

xxxxxxxxxx
can_grasp = (
    USE_DEPTH_CAMERA
    and self.enabled
    and (not self.grasping)
    and (self.cur_distance != 0)
    and (self.align_count >= self.align_required_count)
)

This logic is the most critical behavior switch in the current tutorial:

• Depth camera mode: proceeds with grasping when conditions are met;

• Standard USB camera mode: USE_DEPTH_CAMERA is false, so it only tracks and does not enter the grasping thread.

4.2. Robotic Arm Grasping Execution Dofbot_Track.py

The low-level grasping implementation is located at:

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

In depth camera mode, apriltag_follow.py passes the detected pixel coordinates and depth value to DofbotTrack for end-effector pose calculation and action execution. The core approach is:

1. Convert pixel coordinates + depth to the camera coordinate system position;

2. Combine with the hand-eye matrix and current end-effector pose to convert to the robotic arm world coordinate system;

3. Call inverse kinematics to solve for joint angles;

4. Control the robotic arm to descend, grasp, lift, place, and return to zero.

xxxxxxxxxx
camera_location = self.pixel_to_camera_depth((cx, cy), cz)
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, pose_R = self.mat_to_xyz_euler(WorldPose)

Therefore, the actual execution chain in this section is:

apriltag_follow.py handles detection, centering, and grasping trigger determination;

Dofbot_Track.py handles coordinate transformation, inverse kinematics solving, and robotic arm action execution.

    request.tar_z = pose_T[2]  
    # Specify the service content as ik
    request.kin_name = "ik"
    # Target Roll value of the robotic arm end effector, in radians, representing the current roll value of the arm end
    request.Roll = self.CurEndPos[3] 
    print("calcutelate_request: ",request)
    try:
        response = self.client.call(request)
        joints = [0.0, 0.0, 0.0, 0.0, 0.0,0.0]
        # Assign the joint1-joint6 values returned by the service to joints
        joints[0] = response.joint1 #response.joint1
        joints[1] = response.joint2
        joints[2] = response.joint3
        if response.joint4>90:
            joints[3] = 90
        else:
            joints[3] = response.joint4
        joints[4] = 90 
        joints[5] = 30
        print("compute_joints: ",joints)
        # Calculate the distance from the AprilTag to the robotic arm base coordinate system
        dist = math.sqrt(request.tar_y ** 2 + request.tar_x** 2)
        # If the distance is between 18 cm and 30 cm, control the arm to move to the grasping point
        if dist>0.18 and dist<0.30:
            self.Buzzer()
            print("Clamp Mode.")
            # Execute pubTargetArm function, passing the calculated joints values as parameters
            self.pub_arm(joints)
            time.sleep(3.5)
            # Execute grasping, movement, and placement actions
            self.move()
        else:
            print("Too far or too close.")
    except Exception:
       rospy.loginfo("run error")