Welcome to DOFBOT-Ultra repository

AprilTag Abnormal Height 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 feature, the web control program backend process must be shut down.

This section is only for the depth camera version.

1. Feature Description

After the program starts, the camera detects AprilTags. It will pick up tags whose height exceeds the set threshold (default 50mm) and place them at a designated position.

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 in the native environment.

​x
# Start camera
ros2 launch orbbec_camera dabai_dcw2.launch.py
# Start inverse kinematics
ros2 run dofbot_ultra_info kinematics_dofbotultra
# Start low-level control
ros2 run dofbot_ultra_driver arm_driver
# Start AprilTag detection
ros2 run dofbot_ultra_driver apriltag_list
# Start arm pick-up program for AprilTags
ros2 run dofbot_ultra_driver apriltag_remove_higher

# 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}'

What does the GUI display mean?

Display Item	Meaning	Unit	Purpose
Height	Z-axis height of AprilTag	meters	Determines graspability (>40mm)
Distance	Horizontal distance to arm base	meters	Checks if within working range
FPS	Image processing frame rate	fps	System performance monitoring

• S value: Plane tilt correction coefficients (slope_x, slope_y)

• Z_Off: Z-axis offset correction value

• Off X: X-axis visual offset

• Bias: Robotic arm deviation compensation value

Note: The Height and Distance values may have an error of up to 5mm from the actual values, which does not affect grasping.

If it does affect grasping, recalibration is needed.

2.2. Operation

Click on the image frame with the mouse, then press the Space key on the keyboard. The robotic arm will pick up the AprilTag block whose height exceeds the set threshold and place it at the designated position. After placement, the arm returns to its initial posture.

3. Program Flowchart

3.1. apriltag_list.py

3.2. apriltag_remove_higher.py

4. Core Code Analysis

4.1. apriltag_list.py

Code path (inside Docker):

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

Import necessary libraries

xxxxxxxxxx
import rclpy
from rclpy.node import Node
import numpy as np
from sensor_msgs.msg import Image
from message_filters import ApproximateTimeSynchronizer, Subscriber
# Import library for drawing tags
from dofbot_ultra_driver.vutils import draw_tags
# Import AprilTag library
from dt_apriltags import Detector
from cv_bridge import CvBridge
import cv2 as cv
# Import custom service data type
from dofbot_ultra_interface.srv import Kinematics
# Import custom message data type
from dofbot_ultra_interface.msg import *
from std_msgs.msg import Float32,Bool
encoding = ['16UC1', '32FC1']
import time
# Import transforms3d library for handling 3D space transformations, performing conversions between quaternions, rotation matrices, and Euler angles, supporting 3D geometric operations and coordinate transformations
import transforms3d as tfs
# Import transformations for processing and computing 3D space transformations, including conversions between quaternions and Euler angles
import tf_transformations as tf  
import math

Program parameter initialization, creating publishers, subscribers, and clients

xxxxxxxxxx
def __init__(self):
    super().__init__('apriltag_detect')
    # Publish the initial arm posture, which is also the detection posture
    self.init_joints = [90.0, 120, 0, 0.0, 90, 90]
    # Create two subscribers, subscribing to color 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)
  
    # Time-synchronize the color and depth image subscriptions
    self.ts = ApproximateTimeSynchronizer([self.rgb_image_sub, self.depth_image_sub],queue_size=10,slop=0.5)
    # Create a subscriber for grasping results
    self.subscription = self.create_subscription(Bool,'grasp_done',self.GraspStatusCallback,qos_profile=1)
    # Set the callback function TagDetect for the synchronized messages, connecting the callback with the subscribed messages so it is automatically called on new messages
    self.ts.registerCallback(self.TagDetect)
    # Create bridges for converting color and depth 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 an AprilTag detector object with parameters
    '''
    searchpath: Path to the tag model.
    families: Tag family to use, e.g. 'tag36h11'.
    nthreads: Number of threads for parallel processing to improve detection speed.
    quad_decimate: Downsampling factor for the input image to reduce computation.
    quad_sigma: Standard deviation of Gaussian blur affecting image preprocessing.
    refine_edges: Whether to refine edges for improved detection accuracy.
    decode_sharpening: Sharpening parameter during decoding to enhance tag contrast.
    debug: Debug mode switch to view 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)
    # List storing center x coordinates of AprilTags
    self.Center_x_list = []
    # List storing center y coordinates of AprilTags
    self.Center_y_list = []
    self.heigh = 0.0
    # Position and orientation of the arm end-effector
    self.CurEndPos = [-0.006,0.116261662208,0.0911289015753,-1.04719,-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-effector and camera, describing their relative position and orientation
    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]])

AprilTag information callback function tag_info_callback

xxxxxxxxxx
def TagDetect(self,color_frame,depth_frame):
    #rgb_image
    rgb_image = self.rgb_bridge.imgmsg_to_cv2(color_frame,'rgb8')
    result_image = np.copy(rgb_image)
    #depth_image
    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)
    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 in ascending order by default, no manual sorting needed
    draw_tags(result_image, tags, corners_color=(0, 0, 255), center_color=(0, 255, 0))
    key = cv2.waitKey(10)
    self.Center_x_list = list(range(len(tags)))
    self.Center_y_list = list(range(len(tags)))
    if key == 32:
        self.pubPos_flag = True
    if len(tags) > 0 :
        for i in range(len(tags)):
            center_x, center_y = tags[i].center
            # Store center xy of AprilTag in Center_x_list and Center_y_list
            self.Center_x_list[i] = center_x
            self.Center_y_list[i] = center_y
            cx = center_x
            cy = center_y
            # Compute depth value at center coordinates
            cz = depth_image_info[int(cy),int(cx)]/1000
            # Call compute_heigh function to compute the tag height, passing center coordinates and depth value; returns a position list where pose[2] is the z value (height)
            pose = self.compute_heigh(cx,cy,cz)
            # Scale the height value, convert to millimeters
            heigh = round(pose[2],4)*1000 
            heigh = 'heigh: ' + str(heigh) + 'mm'
            # Compute distance from tag to the base coordinate system, scale and convert to millimeters
            dist_detect = math.sqrt(pose[1] ** 2 + pose[0]** 2)
            dist_detect = round(dist_detect,3)*1000
            dist = 'dist: ' + str(dist_detect) + 'mm'
            # Draw height and distance values on the 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)
            print("Pose: ",pose)
        for i in range(len(tags)):
            if self.pubPos_flag == True:
                tag = AprilTagInfo()
                # Assign message data: id is the tag ID, x and y are the center coordinates, z is the depth value at the center, scaled down by 1000, unit is meters
                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 the distance is not 0, the data is valid, then publish the AprilTag info
                if tag.z!=0:
                    self.tag_info_pub.publish(tag)
                # Change self.pubPos_flag state to prevent repeated publishing, wait for grasping completion before changing state again
                    self.pubPos_flag = False
                else:
                    print("Invalid distance.")
    # Convert color image color space from RGB to BGR                   
    result_image = cv2.cvtColor(result_image, cv2.COLOR_RGB2BGR)
    # Display the image
    cv2.imshow("result_image", result_image)
    key = cv2.waitKey(1)

Height computation function compute_heigh

xxxxxxxxxx
def compute_heigh(self,x,y,z):
    # First coordinate transformation: from pixel coordinate system to camera coordinate system
    camera_location = self.pixel_to_camera_depth((x,y),z)
    #print("camera_location: ",camera_location)
    # Second coordinate transformation: from camera coordinate system to arm end-effector coordinate system
    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)
    # Get current arm end-effector position and orientation
    EndPointMat = self.get_end_point_mat()
    # Third coordinate transformation: from arm end-effector coordinate system to base coordinate system. The resulting WorldPose (rotation transformation matrix) represents the position and orientation of the AprilTag center relative to the arm base coordinate system
    WorldPose = np.matmul(EndPointMat, PoseEndMat) 
    #WorldPose = np.matmul(PoseEndMat,EndPointMat)
    # Convert the rotation transformation matrix to xyz and Euler angles
    pose_T, pose_R = self.mat_to_xyz_euler(WorldPose)
    # Return the position vector pose_T
    return pose_T

4.2. apriltag_remove_higher.py

Code path:

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

Import necessary libraries

xxxxxxxxxx
import math
import rclpy
from rclpy.node import Node
import numpy as np
from std_msgs.msg import Float32,Bool
import time
from dofbot_ultra_interface.msg import *       # Confirm ROS2 message package name matches
from dofbot_ultra_interface.srv import Kinematics
# Import transforms3d library for handling 3D space transformations, performing conversions between quaternions, rotation matrices, and Euler angles, supporting 3D geometric operations and coordinate transformations
import transforms3d as tfs
# Import transformations for processing and computing 3D space transformations, including conversions between quaternions and Euler angles
import tf_transformations as tf   
import threading

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 messages
    self.sub = self.create_subscription(AprilTagInfo,'PosInfo',self.pos_callback,1)
    # Create a publisher for servo target angles to control the arm servos
    self.pub_point = self.create_publisher(ArmJoint, 'TargetAngle',1)
    # Create a publisher for grasping results
    self.pubGraspStatus = self.create_publisher(Bool, 'grasp_done',1)
    # Create a client for the inverse kinematics service, used to compute current arm end-effector position and orientation and solve for target servo values
    self.client = self.create_client(Kinematics, 'dofbot_kinematics')
    # Initial grasp flag: True means graspable, False means not graspable
    self.grasp_flag = True
    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 orientation, corresponding to 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-effector and camera, describing their relative position and orientation
    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 current arm end-effector position and orientation, updates self.CurEndPos
    self.get_current_end_pos()
    # Define current tag ID, used later to place the tag at the corresponding position
    self.cur_tagId = 0
    # Print current arm end-effector position and orientation
    print("Current_End_Pose: ",self.CurEndPos)
    print("Init Done")

AprilTag information callback function tag_info_callback

xxxxxxxxxx
def tag_info_callback(self,msg):
    #print("msg: ",msg)
    pos_x = msg.x
    pos_y = msg.y
    pos_z = msg.z
    self.cur_tagId = msg.id
    # If the received center depth value is > 0, the data is valid
    if pos_z!=0.0:
        print("xyz id : ",pos_x,pos_y,pos_z,self.cur_tagId)
        # Get current arm end-effector position and orientation
        self.get_current_end_pos()
        # First coordinate transformation: from pixel coordinate system to camera coordinate system
        camera_location = self.pixel_to_camera_depth((pos_x,pos_y),pos_z)
        #print("camera_location: ",camera_location)
        # Second coordinate transformation: from camera coordinate system to arm end-effector coordinate system
        PoseEndMat = np.matmul(self.EndToCamMat, self.xyz_euler_to_mat(camera_location, (0, 0, 0)))
        EndPointMat = self.get_end_point_mat()
        # Third coordinate transformation: from arm end-effector coordinate system to base coordinate system. The resulting WorldPose (rotation transformation matrix) represents the position and orientation of the AprilTag center relative to the arm base coordinate system
        WorldPose = np.matmul(EndPointMat, PoseEndMat) 
        # Convert the rotation transformation matrix to xyz and Euler angles
        pose_T, pose_R = self.mat_to_xyz_euler(WorldPose)
        print("pose_T: ",pose_T)
        # Check grasp flag: if True and height > 0.05 (i.e., above 0.05m), execute grasping
        if self.grasp_flag == True and (pose_T[2])>0.05:
            self.grasp_flag = False
            # Start a thread to execute the grasp function, parameter is the computed pose_T (xyz values) representing the target position of the arm end-effector
            grasp = threading.Thread(target=self.grasp, args=(pose_T,))
            grasp.start()
            grasp.join()

Arm grasping function grasp

xxxxxxxxxx
def grasp(self,pose_T):
    print("------------------------------------------------")
    print("pose_T: ",pose_T)
    # Call the inverse kinematics service (ik), assign required request parameters
    request = kinematicsRequest()
    # Target x of arm end-effector in meters. 0.01 is the x-axis (left-right) offset parameter. Due to minor servo differences, the computed position may differ from actual; adjust according to actual conditions
    request.tar_x = pose_T[0]  - 0.01
    # Target y of arm end-effector in meters. 0.015 is the y-axis (front-back) offset parameter. Due to minor servo differences, the computed position may differ from actual; adjust according to actual conditions
    request.tar_y = pose_T[1]  + 0.015
    # Target z of arm end-effector in meters. 0.02 is the z-axis (vertical) offset parameter. Due to minor servo differences, the computed position may differ from actual; adjust according to actual conditions
    request.tar_z = pose_T[2] + request.tar_y* 0.02
    # Specify the service type as ik
    request.kin_name = "ik"
    # Target Roll of arm end-effector in radians, using the current arm end-effector roll
    request.Roll = self.CurEndPos[3]
    print("calcutelate_request: ",request)
    try:
        response = self.client.call(request)
        #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 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)
        # Execute pubTargetArm function, passing the computed joints as parameters
        self.pubTargetArm(joints)
        time.sleep(3.5)
        # Execute move function to pick up block and place it at the set position based on tag ID
        self.move()
    except Exception:
       rospy.loginfo("run error")

Arm target angle publishing function pubTargetArm

xxxxxxxxxx
def pubArm(self, joints, id=1, angle=90, run_time=2000):
    armjoint = ArmJoint()
    armjoint.run_time = run_time
    if len(joints) != 0: armjoint.joints = joints
    else:
        armjoint.id = id
        armjoint.angle = angle
        self.pubPoint.publish(armjoint)

Grasp and place function move

xxxxxxxxxx
def move(self):
    print("self.gripper_joint = ",self.gripper_joint)
    self.pubArm([],5, self.gripper_joint, 2000)
    time.sleep(2.5)
    self.pubArm([],6, 135, 2000)
    time.sleep(2.5)
    self.pubArm([],2, 135, 2000)
    time.sleep(2.5)
    # Change self.down_joint based on tag ID, this value determines the placement position of the AprilTag block
    if self.cur_tagId == 1:
        self.down_joint = [130.0, 55.0, 34.0, 16.0, 90.0,135]
    elif self.cur_tagId == 2:
        self.down_joint = [170.0, 55.0, 34.0, 16.0, 90.0,135]
    elif self.cur_tagId == 3:
        self.down_joint = [50.0, 55.0, 34.0, 16.0, 90.0,135]
    elif self.cur_tagId == 4:
        self.down_joint = [10.0, 55.0, 34.0, 16.0, 90.0,135]
    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 initial position
    self.pubArm(self.init_joints)
    time.sleep(5)
    # After returning to initial position, publish the grasp completion message and change the flag for next grasp
    self.grasp_flag = True
    grasp_done = Bool()
    grasp_done.data = True
    self.pubGraspStatus.publish(grasp_done)