Welcome to DOFBOT-Ultra repository

AprilTag 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 starting this function, the web control program backend process must be closed.

1. Function Description

After the program starts, the camera detects AprilTags, outlines them in the image, and prints the corresponding ID value. Press the space bar to start the sorting program. The robotic arm will grasp the detected AprilTag block and place it at the designated position based on the detected ID.

If the grip is inaccurate, please recalibrate.

The vision and grasping logic in this tutorial automatically switches based on CAMERA_TYPE, but the camera startup 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

Open a terminal and enter the following commands (open multiple terminals; the system will automatically enter the Docker environment). For RDK, run directly on the host machine:

​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 the inverse solution program
ros2 run dofbot_ultra_info kinematics_dofbotultra
# Start the low-level control:
ros2 run dofbot_ultra_driver arm_driver
# AprilTag detection
ros2 run dofbot_ultra_driver apriltag_detect
# Grasping
ros2 run dofbot_ultra_driver grasp

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

2. Except for the camera startup command, the startup method for other nodes is the same.

Can be hand-held or placed on a horizontal tabletop.

2.2. Operation

Use a 3x3 square block (30mm x 30mm x 30mm). Click on the image frame (to give focus to the image GUI), then press the space bar. The robotic arm will automatically select the coordinate calculation method based on the current CAMERA_TYPE:

• Depth Camera Mode: Calculates the grasping point based on the AprilTag center coordinates and the center point depth value.

• Standard USB Camera Mode: Calculates the grasping point based on the AprilTag center pixel coordinates and the completed planar calibration results.

The robotic arm will then lower its gripper to grasp the detected AprilTag block. After grasping, it places the block at the designated position based on the detected ID. After placement, it returns to the detection posture to detect the next AprilTag.

Information displayed in the grasping terminal:

Output data analysis:

System Initialization Phase

xxxxxxxxxx
[DofbotConfig] Loaded offsets from .../offset_value.yaml
Bias: x=-0.015, y=0.0, z=-0.015
Comp: x=0.07, z=-0.08
slope_x: -0.0621524570603543, slope_y: -0.03930853176041961

Loaded 4 calibration configuration files:

• Fixed Offset: Systematic error in visual detection

• Arm Bias: Physical error in robotic arm movement (-1.5cm in X/Z directions)

• Plane Tilt Correction: Worktable tilt compensation coefficients

• Height Dynamic Compensation: Dynamically adjusts X/Z compensation based on height

xxxxxxxxxx
Current_End_Pose: [0.1198, 0.0006, 0.0914, 3.124, 1.047, 0.000]

Initial End-Effector Pose: Current robotic arm position (X=11.98cm, Y=0.06cm, Z=9.14cm)

Visual Detection

xxxxxxxxxx
xyz id : 1 pos : 351.748, 372.711, 0.188

Detected AprilTag ID=1:

• Pixel coordinates: (351.7, 372.7)

• Distance: 18.8cm

xxxxxxxxxx
Coord Raw: x=0.165, y=-0.012, z=0.004

Raw World Coordinates (uncorrected): Object position (16.5cm, -1.2cm, 0.4cm)

Coordinate Correction Detailed Process

1️⃣ Plane Tilt Correction

xxxxxxxxxx
Tilt Correction: SlopeX=-0.0622, SlopeY=-0.0393 -> Correction=-0.0098

Compensation: Due to table tilt (possible) and visual errors (inevitable), the position X=16.5cm, Y=-1.2cm requires +0.98cm height compensation.

2️⃣ Height Dynamic Compensation

xxxxxxxxxx
Height Comp: Z_Comp_X=0.07 * 0.0035 = 0.0002
Height Comp: Z_Comp_Z=-0.08 * 0.0035 = -0.0003

Behavior: When grasping at a higher position, the end-effector tends to extend forward 0.02mm and droop 0.03mm, requiring compensation.

3️⃣ Final Coordinate Calculation

xxxxxxxxxx
Final X: 0.1651 + 0.0165(Offset) + (-0.0150)(Bias) - 0.0002 = 0.1663
Final Z: 0.0035 + 0.0008 + (-0.0150) - (-0.0098) - (-0.0003) = -0.0006

Final Target: [16.63cm, 0.21cm, -0.06cm]

xxxxxxxxxx
✅ Z=-0.06cm >= -8cm This is a safety limit to prevent the arm from grasping too low.

Inverse Kinematics Solution

xxxxxxxxxx
Grasping target: x=0.166, y=0.002, z=-0.001
Target Orientation: r=0.000, p=-1.570, y=0.013

Grasp Command:

• Position: Approaching the tabletop grasping posture

• Orientation: Yaw adapts to the target angle

xxxxxxxxxx
calcutelate_response: joint1=90.5°, joint2=129.4°, joint3=156.4°, joint4=153.5°, joint5=95.8°
[90.5, 50.6, 23.6, 26.5, 95.8, 30]

After successful inverse kinematics solution, 6 joint angles are obtained, and the robotic arm starts moving.

Grasp Sequence Execution

xxxxxxxxxx
self.cur_tagId 1 → self.down_joint [155.0, 35, 70, 5, 60.0, 142]
Grasp sequence completed.

Complete Grasp Workflow:

1. ✅ Move to target (1 second)

2. ✅ Close gripper (1.5 seconds)

3. ✅ Raise arm (1.8 seconds)

4. ✅ Move to Tag1 drop-off position

5. ✅ Release gripper

6. ✅ Return to initial position

3. Program Flowchart

apriltag_detect.py

grasp.py

4. Core Code Analysis

4.1. apriltag_detect.py

Code path:

xxxxxxxxxx
Raspberry Pi:/home/yahboom/dofbot_ultra_ws/src/dofbot_ultra_driver/dofbot_ultra_driver/apriltag_detect.py
RDK:/home/sunrise/dofbot_ultra_ws/src/core/dofbot_ultra_driver/dofbot_ultra_driver/apriltag_detect.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 message_filters import ApproximateTimeSynchronizer, Subscriber
from std_msgs.msg import Float32, Bool
from cv_bridge import CvBridge
import cv2 as cv
from dt_apriltags import Detector
import threading
# Import AprilTag drawing library
from dofbot_ultra_driver.vutils import draw_tags
# Import custom service data type
from dofbot_ultra_interface.srv import Kinematics
# Import custom message data types
from dofbot_ultra_interface.msg import *
import pyzbar.pyzbar as pyzbar
from std_msgs.msg import Float32,Bool
import time
import queue

Program parameter initialization, creating publishers and subscribers:

xxxxxxxxxx
def __init__(self):
    rospy.init_node('apriltag_detect')
    #self.init_joints = [90.0, 120, 0, 0.0, 90, 90]
    # Publish the initial posture of the arm, which is also the detection posture
    self.init_joints = [90.0, 120, 0.0, 0.0, 90, 30]
    # Create two subscribers for color image and depth image topics
    self.depth_image_sub = Subscriber(self, Image, '/camera/depth/image_raw')
    self.rgb_image_sub = Subscriber(self, Image, '/camera/color/image_raw')
    # Create a publisher for AprilTag information
    self.pos_info_pub = self.create_publisher(AprilTagInfo, "PosInfo", qos_profile=10)
    # Create a publisher for the arm's target angle
    self.pubPoint = self.create_publisher(ArmJoint, "TargetAngle", qos_profile=1)
    # Time-synchronize the color and depth image subscriptions
    self.TimeSynchronizer = message_filters.ApproximateTimeSynchronizer([self.rgb_image_sub,self.depth_image_sub],1,0.5)
    # Create a subscriber for grasping results
    self.subscription = self.create_subscription(Bool,'grasp_done',self.GraspStatusCallback,qos_profile=1)
    # Register the TagDetect callback for synchronized messages
    self.ts.registerCallback(self.TagDetect)
    # Create bridges to convert image topic messages to image data
    self.rgb_bridge = CvBridge()
    self.depth_bridge = CvBridge()
    # Flag for publishing AprilTag info, publishes /TagInfo topic data when True
    self.pubPos_flag = False
    # Create the AprilTag detector object with parameters:
    '''
    searchpath: Path to look for tag models.
    families: Tag family to use, e.g. 'tag36h11'.
    nthreads: Number of parallel processing threads for faster detection.
    quad_decimate: Downscale factor for the input image to reduce computation.
    quad_sigma: Standard deviation for Gaussian blur in image preprocessing.
    refine_edges: Whether to refine edges for higher detection accuracy.
    decode_sharpening: Sharpening parameter for decoding, enhances tag contrast.
    debug: Debug mode switch for viewing detection process information.
    '''
    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)
   

Key focus on the TagDetect callback function:

xxxxxxxxxx
def 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])
    # Convert depth image to pseudo-color image
    depth_to_color_image = cv.applyColorMap(cv.convertScaleAbs(depth_image, alpha=0.03), cv.COLORMAP_JET)
    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): Convert RGB image to grayscale for tag detection.
    False: Do not estimate tag pose.
    None: No camera parameters provided, possibly for simple detection only.
    0.025: Tag size (in meters) to assist the detection algorithm.
    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 detected tags (optional step)
    tags = sorted(tags, key=lambda tag: tag.tag_id) 
    # Call draw_tags to draw detected AprilTag info on the color image (corners, center, ID)
    draw_tags(result_image, tags, corners_color=(0, 0, 255), center_color=(0, 255, 0))
    # Wait for keyboard input, 32 = space bar, changes self.pubPos_flag to allow publishing tag info
    key = cv2.waitKey(10)
    if key == 32:
        self.pubPos_flag = True
    # Check if tags are detected (len > 0)
    if len(tags) > 0 :
        # Iterate through detected tags
        for i in range(len(tags)):
            if self.pubPos_flag == True:
                # Get the center of the detected tag
                center_x, center_y = tags[i].center
                # Mark the center point on the pseudo-color image
                cv.circle(result_image, (int(center_x),int(center_y)), 10, (0,210,255), thickness=-1)
                # Create AprilTag info message
                tag = AprilTagInfo()
                # Assign values: id = tag ID, x and y = center coordinates, z = depth value at center (scaled down by 1000, in meters)
                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
                # Print detected tag info
                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)
                # If tag distance > 0, it is valid data; publish the tag info message
                if tag.z>0:
                    self.tag_info_pub.publish(tag)
                    # Change self.pubPos_flag to prevent multiple publishes; wait for grasp completion to reset
                    self.pubPos_flag = False
                else:
                    print("Invalid distance.")
    # Convert color image from RGB to BGR
    result_image = cv2.cvtColor(result_image, cv2.COLOR_RGB2BGR)
    # Display images
    cv2.imshow("result_image", result_image)
    cv2.imshow("depth_image", depth_to_color_image)
    key = cv2.waitKey(1)

Grasp status callback function:

xxxxxxxxxx
def GraspStatusCallback(self,msg):
    # If received message data is True, grasping is complete; set self.pubPos_flag to allow publishing next frame
    if msg.data == True:
        self.pubPos_flag = True

4.2. grasp.py

Code path:

xxxxxxxxxx
Raspberry Pi:/home/yahboom/dofbot_ultra_ws/src/dofbot_ultra_driver/dofbot_ultra_driver/grasp.py
RDK:/home/sunrise/dofbot_ultra_ws/src/core/dofbot_ultra_driver/dofbot_ultra_driver/grasp.py

Import necessary library files:

xxxxxxxxxx
import rclpy
from rclpy.node import Node
import numpy as np
from std_msgs.msg import Float32, Bool
import time
import math
from dofbot_ultra_interface.msg import *      
from dofbot_ultra_interface.srv import Kinematics
# Import transforms3d library for handling 3D transformations, quaternions, rotation matrices, and Euler angles
import transforms3d as tfs
# Import transformations library for 3D transformations including quaternions and Euler angles
import tf_transformations as tf         # ROS2 uses tf_transformations
import threading
from ament_index_python import get_package_share_directory
import yaml
import os
from Arm_Lib import Arm_Device

Open the offset parameter file:

xxxxxxxxxx
pkg_path = get_package_share_directory('dofbot_ultra_driver')
offset_file = os.path.join(pkg_path,'config', 'offset_value.yaml')

with open(offset_file, 'r') as file:
    offset_config = yaml.safe_load(file)
print(offset_config)
print("----------------------------")
print("x_offset: ",offset_config.get('x_offset'))
print("y_offset: ",offset_config.get('y_offset'))
print("z_offset: ",offset_config.get('z_offset'))

Program parameter initialization, creating publishers, subscribers, and clients:

xxxxxxxxxx
def __init__(self):
    super().__init__('color_grap')
    # Create a subscriber for the TagInfo topic to receive AprilTag information
    self.sub = self.create_subscription(AprilTagInfo,'PosInfo',self.pos_callback,1)
    # Create a publisher for the servo target angle topic to control the arm
    self.pub_point = self.create_publisher(ArmJoint, 'TargetAngle',1)
    # Create a publisher for the grasping result topic
    self.pubGraspStatus = self.create_publisher(Bool, 'grasp_done',1)
    # Create a client for requesting the inverse solution service to calculate end-effector pose and joint values
    self.client = self.create_client(Kinematics, 'dofbot_kinematics')
    # Initial grasping flag, True = can grasp, False = cannot grasp
    self.grasp_flag = True
    # Arm initial posture
    self.init_joints = [90.0, 120, 0.0, 0.0, 90, 90]
    self.down_joint = [130.0, 55.0, 34.0, 16.0, 90.0,125]
    self.gripper_joint = 90
    # Initialize current end-effector position and pose (x, y, z, roll, pitch, yaw)
    self.CurEndPos = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    # Depth camera intrinsic parameters
    self.camera_info_K = [477.57421875, 0.0, 319.3820495605469, 0.0, 477.55718994140625, 238.64108276367188, 0.0, 0.0, 1.0]
    # Rotation/transformation matrix between arm end and camera, describing relative position and pose
    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]])
    # Get the current end-effector position and pose, updates self.CurEndPos
    self.get_current_end_pos()
    # Define the current tag ID, used for placing the block at the corresponding position
    self.cur_tagId = 0
    # Read offset parameter file and assign to offset variables
    self.x_offset = offset_config.get('x_offset')
    self.y_offset = offset_config.get('y_offset')
    self.z_offset = offset_config.get('z_offset')
    # Print current end-effector position and pose
    print("Current_End_Pose: ",self.CurEndPos)
    print("Init Done")

AprilTag info callback function tag_info_callback:

xxxxxxxxxx
def tag_info_callback(self,msg):
    pos_x = msg.x
    pos_y = msg.y
    pos_z = msg.z
    self.cur_tagId = msg.id
    # Check if the received center depth is > 0, indicating valid data
    if pos_z!=0.0:
        print("xyz id : ",pos_x,pos_y,pos_z,self.cur_tagId)
        # First coordinate transformation: pixel coordinates to camera coordinates
        camera_location = self.pixel_to_camera_depth((pos_x,pos_y),pos_z)
        #print("camera_location: ",camera_location)
        # Second coordinate transformation: camera coordinates to arm end coordinates
        PoseEndMat = np.matmul(self.EndToCamMat, self.xyz_euler_to_mat(camera_location, (0, 0, 0)))
        #PoseEndMat = np.matmul(self.xyz_euler_to_mat(camera_location, (0, 0, 0)),self.EndToCamMat)
        EndPointMat = self.get_end_point_mat()
        # Third coordinate transformation: arm end coordinates to base coordinates
        # The resulting WorldPose (rotation matrix) is the AprilTag center's position and pose relative to the arm base
        WorldPose = np.matmul(EndPointMat, PoseEndMat) 
        #WorldPose = np.matmul(PoseEndMat,EndPointMat)
        # Convert rotation matrix to xyz and Euler angles
        pose_T, pose_R = self.mat_to_xyz_euler(WorldPose)
        # Add offset parameters to compensate for servo value discrepancies
        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
        print("pose_T: ",pose_T)
        # Check grasping flag, True = proceed with grasping
    if self.grasp_flag == True :
        print("Take it now.")
        # Change the flag to prevent re-grasping during the current grasp operation
        self.grasp_flag = False
        # Start a thread to execute the grasp program with the calculated pose_T (xyz values)
        grasp = threading.Thread(target=self.grasp, args=(pose_T,))
        # Start the thread
        grasp.start()
        grasp.join()

Arm grasping function grasp:

xxxxxxxxxx
def grasp(self,pose_T):
    print("------------------------------------------------")
    print("pose_T: ",pose_T)
    # Call the inverse solution service (IK), assign the required request parameters
    request = kinematicsRequest()
    # Target x position of the arm end, in meters
    request.tar_x = pose_T[0] -0.01
    # Target y position of the arm end, in meters
    request.tar_y = pose_T[1] 
    # Target z position of the arm end, in meters; 0.2 is a scaling factor, adjust as needed
    request.tar_z = pose_T[2] +  (math.sqrt(request.tar_y**2+request.tar_x**2)-0.181)*0.2
    # Set the service type to ik
    request.kin_name = "ik"
    # Target Roll of the arm end, in radians; uses the current arm end roll value
    request.Roll = self.CurEndPos[3] 
    print("calcutelate_request: ",request)
    try:
        
        future = self.client.call_async(request)
        rclpy.spin_until_future_complete(self, future, timeout_sec=5.0) 
        response = future.result()
        #print("calcutelate_response: ",response)
        joints = [0.0, 0.0, 0.0, 0.0, 0.0,0.0]
        # Assign the joint1-joint6 values returned by the service
        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)
        # Call pubTargetArm with the calculated joint values
        self.pubTargetArm(joints)
        time.sleep(3.5)
        # Call move to place the block based on the detected tag ID
        self.move()

        except Exception:
            rospy.loginfo("run error")

Function to publish arm target angles pubTargetArm:

xxxxxxxxxx
def pubTargetArm(self, joints, id=6, angle=180.0, runtime=2000):
        print(joints)      self.Arm.Arm_serial_servo_write6(joints[0],joints[1],joints[2],joints[3],joints[4],joints[5],2000)

Grasping and placement function move:

def move(self):
    print("self.gripper_joint = ",self.gripper_joint)
    self.pubArm([],5, self.gripper_joint, 2000)
    time.sleep(2.5)
    self.pubArm([],6, 140, 2000)
    time.sleep(2.5)
    self.pubArm([],2, 120, 2000)
    time.sleep(2.5)
    # Change self.down_joint based on tag ID, representing the placement position
    if self.cur_tagId == 1:
        self.down_joint = [150.0, 30, 70, 5, 90.0,140]
    elif self.cur_tagId == 2:
        self.down_joint = [180.0, 35, 60, 0, 90.0,140]
    elif self.cur_tagId == 3:
        self.down_joint = [28.0, 30, 70, 2, 90.0,140]
    elif self.cur_tagId == 4:
        self.down_joint = [0.0, 43, 48, 6, 90.0,140]
    self.pubArm(self.down_joint)
    time.sleep(2.5)
    self.pubArm([],6, 90, 2000)
    time.sleep(2.5)
    self.pubArm([],2, 90, 2000)
    time.sleep(2.5)
    # After placement, return to the initial position
    self.pubArm(self.init_joints)
    time.sleep(5)
    # Wait for the arm to return to its initial position, then publish grasp completion message
    self.grasp_flag = True
    grasp_done = Bool()
    grasp_done.data = True
    self.pubGraspStatus.publish(grasp_done)