Welcome to DOFBOT-Ultra repository
10. Yolov26 garbage sorting
YOLOv26 Garbage 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.
1. Feature Description
After the program starts, the camera captures images. Place a garbage label tag block in the image. The robotic arm detects the category of the garbage label tag block, picks it up, and places it at the designated position according to its category. After placement, the arm returns to the detection posture.
The garbage detection and sorting 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.pyStandard USB Camera: use
usb_cam.launch.pyBoth modes retain the "detect then grasp and sort" functionality:
DaBai Depth Camera: computes the grasp point via the depth pipeline;
Standard USB Camera: computes the grasp point via planar calibration results.
2. Startup and Operation
2.1. Startup Commands
Enter the following commands in the terminal to start:
This section optionally uses either YOLOv11 or YOLOv26 models for inference. Official data shows that the YOLOv26 model significantly outperforms YOLOv11 in both accuracy and CPU inference speed. The code file names below use "yolo11", but the actual model loaded depends only on the startup parameters.
Tests show that the YOLOv26 model achieves a higher detection success rate than YOLOv11.
x# DaBai Depth Cameraros2 launch orbbec_camera dabai_dcw2.launch.py
# Standard USB Cameraros2 launch dofbot_ultra_info usb_cam.launch.py
ros2 run dofbot_ultra_driver arm_driverros2 run dofbot_ultra_info kinematics_dofbotultraros2 run dofbot_ultra_yolo msgToimg# Raspberry Piros2 run dofbot_ultra_yolo yolo_detect --ros-args -p model:=best_yolo26.pt -p imgsz:=320# RDK: (RDK uses BPU acceleration)ros2 run dofbot_ultra_yolo yolo_detect# To use the YOLOv11 version, use the following command#RDK:ros2 run dofbot_ultra_yolo yolo_detect --ros-args -p model_path:=/home/sunrise/ultralytics/ultralytics/best_bayese_640x640_nv12.bin#Raspberry Pi:ros2 run dofbot_ultra_yolo yolo_detect --ros-args -p model:=best.pt -p imgsz:=320ros2 run dofbot_ultra_yolo yolo_sortation# 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 must be selected according to the actual camera type.
All other nodes start in the same way regardless of the camera type.
Due to performance differences between mainboards, the time required to load the YOLO detection program may vary. Please wait patiently.
2.2. Operation Procedure
After the program starts, place the block with the garbage label tag in the center of the image. The block must be properly aligned and the icon must face the same direction as the robotic arm (forward, Y-axis direction). Press the Space key to start detection. The robotic arm automatically selects the grasp coordinate computation method corresponding to the current camera mode based on the detected garbage category, then picks up the block and places it at the designated position.
3. Program Flowchart
yolov11.py
yolov11_sortation.py
4. Core Code Analysis
4.1. msgToimg.py
Code path:
xxxxxxxxxxRaspberry Pi:/home/yahboom/dofbot_ultra_ws/src/dofbot_ultra_yolo/dofbot_ultra_yolo/msgToimg.pyRDK:/home/sunrise/dofbot_ultra_ws/src/3d/dofbot_ultra_yolo/dofbot_ultra_yolo/msgToimg.py
Import necessary libraries
xxxxxxxxxximport sysimport rospyimport numpy as npimport osfrom sensor_msgs.msg import Imagefrom cv_bridge import CvBridge, CvBridgeErrorfrom dofbot_ultra_info.msg import Image_MsgProgram parameter initialization, creating publishers and subscribers
xxxxxxxxxxdef __init__(self): # Create bridge for converting color image topic messages to image data self.bridge = CvBridge() # Create color image topic subscriber self.image_sub = rospy.Subscriber("/camera/color/image_raw",Image,self.image_sub_callback) # Create image data publisher self.image_pub = rospy.Publisher('/image_data', Image_Msg, queue_size=1) self.img = np.zeros((480, 640, 3), dtype=np.uint8) # Initial image self.yolov5_img = np.zeros((480, 640, 3), dtype=np.uint8) # Initial image self.img_flip = rospy.get_param("~img_flip", False) # Initialize image data message object self.image = Image_Msg()image_sub_callback callback function
xxxxxxxxxxdef image_sub_callback(self, data): # Receive color image topic message, convert message data to image data self.img = self.bridge.imgmsg_to_cv2(data, "bgr8") # Get image dimensions size = self.img.shape # Assign values to image message object data self.image.height = size[0] # 480 self.image.width = size[1] # 640 self.image.channels = size[2] # 3 self.image.data = data.data # image_data # Publish image topic self.image_pub.publish(self.image)
4.2. yolov11.py
Code path:
xxxxxxxxxxRaspberry Pi:/home/yahboom/dofbot_ultra_ws/src/dofbot_ultra_yolo/dofbot_ultra_yolo/yolo.pyRDK:/home/sunrise/dofbot_ultra_ws/src/3d/dofbot_ultra_yolo/dofbot_ultra_yolo/yolo.py
Import necessary libraries (Raspberry Pi example)
xxxxxxxxxximport rclpyfrom rclpy.node import Nodeimport Arm_Libimport osimport timeimport cv2 import cv2 as cvimport numpy as npimport threadingfrom time import sleepimport ipywidgets as widgetsfrom std_msgs.msg import Float32, Boolfrom IPython.display import displayfrom dofbot_ultra_yolo.fps import FPSfrom ultralytics import YOLOfrom dofbot_ultra_yolo.robot_controller import Robot_Controllerfrom dofbot_ultra_interface.msg import * encoding = ['16UC1', '32FC1']Program parameter initialization, creating publishers and subscribers
xxxxxxxxxxdef __init__(self): super().__init__('detect_node')
self.pr_time = 0 self.image_sub = self.create_subscription(ImageMsg,"/image_data",self.image_sub_callback,qos_profile=1) self.img = np.zeros((480, 640, 3), dtype=np.uint8) # Initial image self.init_joints = [90.0, 120.0, 0.0, 0.0, 90.0, 90.0] self.pubPoint = self.create_publisher(ArmJoint, "TargetAngle", 10) self.pubDetect = self.create_publisher(Yolov11Detect, "Yolov11DetectInfo", 10) self.pub_SortFlag = self.create_publisher(Bool, 'sort_flag', 10) self.grasp_status_sub = self.create_subscription(Bool,'grasp_done',self.GraspStatusCallback,qos_profile=1) self.start_flag = False self.yolo_model = YOLO("/home/yahboom/dofbot_ultra_ws/src/dofbot_ultra_yolo/dofbot_ultra_yolo/best.engine", task='detect') self.fps = FPS()Image topic subscription callback function
xxxxxxxxxx def image_sub_callback(self,data): image = np.ndarray(shape=(data.height, data.width, data.channels), dtype=np.uint8, buffer=data.data) # Convert custom image message to image self.img[:,:,0],self.img[:,:,1],self.img[:,:,2] = image[:,:,2],image[:,:,1],image[:,:,0] # Convert RGB to OpenCV BGR order # self.img = cv2.cvtColor(image, cv2.COLOR_RGB2BGR)
results = self.yolo_model(self.img, save=False, verbose=False) # Use YOLO11 for object detection annotated_frame = results[0].plot( labels = True, # Show labels conf = False, # Show confidence boxes = True, # Draw bounding boxes ) boxes = results[0].boxes key = cv2.waitKey(10)
if boxes != [None] and self.start_flag == True: for box in boxes: # detections per image x_min, y_min, x_max, y_max = map(int, box.xyxy[0]) class_id = int(box.cls) confidence = float(box.conf) label = f"{self.yolo_model.names[class_id]} {confidence:.2f}" # Compute center position center_x = (x_min + x_max) // 2 center_y = (y_min + y_max) // 2
center = Yolov11Detect() center.centerx = float(center_x) center.centery = float(center_y) center.result = str(self.yolo_model.names[class_id])
# cv2.circle(annotated_frame, (center_x, center_y), 5, (0, 0, 255), -1) # Draw red dot cv2.putText(annotated_frame, label, (x_min, y_min - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2) # Display at top-left
self.pubDetect.publish(center) self.start_flag = False cur_time = time.time() fps = str(int(1/(cur_time - self.pr_time))) self.pr_time = cur_time cv2.putText(annotated_frame, fps, (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) cv2.imshow("frame", annotated_frame)4.3. yolov11_sortation.py
Code path:
xxxxxxxxxx/home/yahboom/dofbot_ultra_ws/src/dofbot_ultra_yolo/dofbot_ultra_yolo/yolo_sortation.py
Import necessary libraries
xxxxxxxxxximport rclpyimport cv2from rclpy.node import Nodeimport numpy as npfrom std_msgs.msg import Float32,Bool,Int8from cv_bridge import CvBridgefrom sensor_msgs.msg import Imageimport cv2 as cvimport timeimport math
from message_filters import ApproximateTimeSynchronizer
from dofbot_ultra_interface.msg import * from dofbot_ultra_interface.srv import *
import transforms3d as tfsimport tf_transformations as tf # ROS2 uses tf_transformationsimport threading
from ament_index_python import get_package_share_directoryimport yamlimport osfrom Arm_Lib import Arm_DeviceLoad offset parameter table
xxxxxxxxxxpkg_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'))encoding = ['16UC1', '32FC1']Program parameter initialization, creating publishers, subscribers, etc.
xxxxxxxxxx def __init__(self): super().__init__('yolo_grap') self.cx = 0 self.cy = 0
self.declare_parameter('depth_topic', '/camera/depth/image_raw') self.declare_parameter('depth_width', 640) self.declare_parameter('depth_height', 480) self.depth_topic = str(self.get_parameter('depth_topic').value or '/camera/depth/image_raw') self.depth_width = int(self.get_parameter('depth_width').value) self.depth_height = int(self.get_parameter('depth_height').value) if self.depth_width <= 0: self.depth_width = 640 if self.depth_height <= 0: self.depth_height = 480
self.Arm = Arm_Device() self.sub_joint5 = self.create_subscription(Float32, "adjust_joint5",self.get_joint5Callback,qos_profile=1) # Publisher modifications self.pubPoint = self.create_publisher(ArmJoint, "TargetAngle", qos_profile=10 ) self.pubGraspStatus = self.create_publisher(Bool, "grasp_done", qos_profile=10) self.pub_playID = self.create_publisher(Int8, "player_id", qos_profile=10) # Other subscribers self.subDetect = self.create_subscription(Yolov5Detect, "YoloDetectInfo",self.getDetectInfoCallback,qos_profile=10) self.depth_image_sub = self.create_subscription(Image, self.depth_topic, self.getDepthCallback, qos_profile=1) self.sub_SortFlag = self.create_subscription(Bool,'sort_flag',self.getSortFlagCallback,qos_profile=10) # Service client modifications self.client = self.create_client(Kinematics, "dofbot_kinematics")
self.color_x = 0.0 self.color_y = 0.0 self.color_z = 0.15 self.grasp_flag = True self.init_joints = [90.0, 120.0, 0.0, 0.0, 90.0, 30.0] self.down_joint = [130.0, 55.0, 34.0, 16.0, 90.0,125.0] self.set_joint = [90.0, 120.0, 0.0, 0.0, 90.0, 143.0] self.gripper_joint = 90.0 self.depth_bridge = CvBridge() self.start_sort = False self.CurEndPos = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] self.camera_info_K = [477.0970458984375, 0.0, 320.8152160644531, 0.0, 476.8221435546875, 238.25753784179688, 0.0, 0.0, 1.0] self.EndToCamMat = load_handeye_matrix()
self._pose_refresh_lock = threading.Lock() self._pose_refresh_inflight = False
self.name = None
self.x_offset = global_config.x_offset self.y_offset = global_config.y_offset self.z_offset = global_config.z_offset self.play_id = Int8() self.recyclable_waste=['Newspaper','Zip_top_can','Book','Old_school_bag'] self.toxic_waste=['Syringe','Expired_cosmetics','Used_batteries','Expired_tablets'] self.wet_waste=['Fish_bone','Egg_shell','Apple_core','Watermelon_rind'] self.dry_waste=['Toilet_paper','Peach_pit','Cigarette_butts','Disposable_chopsticks'] self.Arm.Arm_serial_servo_write6_array(self.init_joints, 1000)
time.sleep(1.2) current_joints = [] for i in range(1, 6): angle = self.Arm.Arm_serial_servo_read(i) if angle is None: angle = self.init_joints[i - 1] current_joints.append(float(angle)) self.get_current_end_pos(current_joints)
print("Current_End_Pose: ",self.CurEndPos) print("Init Done") Garbage label detection result callback function getDetectInfoCallback
xxxxxxxxxxdef getDetectInfoCallback(self,msg): # Assign label center coordinates and label name self.cx = int(msg.centerx) self.cy = int(msg.centery) self.name = msg.resultDepth image topic callback function getDepthCallback
xxxxxxxxxxdef getDepthCallback(self,msg): # Process received depth image topic message depth_image = self.depth_bridge.imgmsg_to_cv2(msg, encoding[1]) frame = cv.resize(depth_image, (640, 480)) depth_image_info = frame.astype(np.float32) # Check if self.cy and self.cx are both non-zero if self.cy!=0 and self.cx!=0: # Get depth value at center point self.dist = depth_image_info[self.cy,self.cx]/1000 print("self.dist",self.dist) print("get the cx,cy",self.cx,self.cy) print("get the detect result",self.name) # Check if depth value is non-zero and self.name is not None if self.dist!=0 and self.name!=None: # Check if self.start_sort is True if self.start_sort == True: # Begin coordinate transformation, finally output the position of the center point in the world coordinate system camera_location = self.pixel_to_camera_depth((self.cx,self.cy),self.dist) 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() WorldPose = np.matmul(EndPointMat, PoseEndMat) #WorldPose = np.matmul(PoseEndMat,EndPointMat) pose_T, pose_R = self.mat_to_xyz_euler(WorldPose) # Add offset parameters to compensate for servo value differences 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 # Start grasping thread, passing the computed label position in the world coordinate system grasp = threading.Thread(target=self.grasp, args=(pose_T,)) grasp.start() grasp.join()Sorting start topic callback function getSortFlagCallback
xxxxxxxxxxdef getSortFlagCallback(self,msg): # Check if the received value is True; if so, set self.start_sort to True if msg.data == True: self.start_sort = TrueGrasping function grasp
xxxxxxxxxxdef grasp(self,pose_T): print("------------------------------------------------") print("pose_T: ",pose_T) # Call the ik algorithm in the inverse kinematics service to compute the 6 servo values request = kinematicsRequest() # Target x of arm end-effector in meters request.tar_x = pose_T[0] # Target y of arm end-effector in meters request.tar_y = pose_T[1] # Target z of arm end-effector in meters, 0.2 is the scaling factor; adjust per实际情况 request.tar_z = pose_T[2] + (math.sqrt(request.tar_y**2+request.tar_x**2)-0.181)*0.2 # 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) 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 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) self.pubTargetArm(joints) time.sleep(2.5) # After grasping, call the move function. Based on self.name, determine which garbage category it belongs to and place it at the designated position self.move() except Exception: rospy.loginfo("run error")