Welcome to DOFBOT-Ultra repository

Block 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 function, you need to close the web control program backend process.

This tutorial uses 30*30 wooden blocks for operation, with no height limit.

This section is only for use with the depth camera version.

1. Function Description

After the program starts, place blocks of the same color. The robotic arm will perform color recognition based on the set HSV values. After pressing the space bar, the robotic arm will lower its claw to grasp the recognized color blocks that exceed the height threshold. After placement, it will return to the recognition posture.

2. Startup and Operation

2.1. Startup Commands

Enter the following commands in the terminal to start:

​x
# Start camera:
ros2 launch orbbec_camera dabai_dcw2.launch.py
# Start low-level control:
ros2 run dofbot_ultra_driver arm_driver
# Start inverse kinematics program:
ros2 run dofbot_ultra_info kinematics_dofbotultra
# Start color recognition program:
ros2 run dofbot_ultra_color AnomalyHeightRemoval
# Start robotic arm grasping program:
ros2 run dofbot_ultra_driver grasp 

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

Simplified version

xxxxxxxxxx
# Camera + low-level + inverse kinematics
ros2 launch dofbot_ultra_driver base_arm_kin.launch.py
# Start color recognition program:
ros2 run dofbot_ultra_color AnomalyHeightRemoval
# Start robotic arm grasping program:
ros2 run dofbot_ultra_driver grasp 

2.2. Operation Flow

After starting the terminal, place blocks of different colors. When the camera captures an image, use the following keys to process the image:

• [i] or [I]: Enter color recognition mode, directly load the HSV values calibrated in the previous program for recognition;

• [r] or [R]: Reset program parameters, enter color selection mode, use the mouse to select a region of the block, obtain the HSV values of this region, and release the mouse to enter color recognition mode;

• Space bar: Start grasping color blocks with abnormal height.

As shown in the figure below, when the left image (binary) only displays the uniquely recognized color, click on the color image frame and press the space bar. The robotic arm will lower its claw to grasp the color blocks with abnormal height.

3. Program Flowchart

AnomalyHeightRemoval.py

4. Core Code Analysis

4.1. AnomalyHeightRemoval.py

Code path:

xxxxxxxxxx
Raspberry Pi:/home/yahboom/dofbot_ultra_ws/src/dofbot_ultra_color/dofbot_ultra_color/AnomalyHeightRemoval.py
RDK:/home/sunrise/dofbot_ultra_ws/src/3d/dofbot_ultra_color/dofbot_ultra_color/AnomalyHeightRemoval.py

Import necessary libraries,

xxxxxxxxxx
import cv2
import rclpy
from rclpy.node import Node
import numpy as np
from message_filters import ApproximateTimeSynchronizer, Subscriber
from sensor_msgs.msg import Image
from std_msgs.msg import Float32, Bool
from cv_bridge import CvBridge
import cv2 as cv

import time
import math
import os
encoding = ['16UC1', '32FC1']
import tf_transformations as tf
import transforms3d as tfs

from dofbot_ultra_color.height_measurement import *
from dofbot_ultra_interface.msg import *

Initialize program parameters, create publishers, subscribers,

xxxxxxxxxx
def __init__(self):
    super().__init__('remove_too_heigher')
    self.declare_param()
    self.target_servox=90
    self.window_name = "depth_image"
    self.target_servoy=45
    self.init_joints = [90.0, 120.0, 0.0, 0.0, 90.0, 90.0]
    self.detect_flag = False
    self.compute_height = True
    self.CX_list = []
    self.CY_list = []
    self.R_list = []   
    self.joint5 = Int16()
    self.pubPoint = self.create_publisher(ArmJoint, "TargetAngle", 1)
    self.grasp_status_sub = self.create_subscription(Bool, 'grasp_done', self.GraspStatusCallback, 1)
    self.client = self.create_client(Kinematics, 'dofbot_kinematics')

    # ROS2 subscriber (message synchronization)
    self.depth_image_sub  = Subscriber(self, Image, "/camera/color/image_raw", qos_profile=1)
    self.rgb_image_sub = Subscriber(self, Image, "/camera/depth/image_raw", qos_profile=1)
    self.TimeSynchronizer = ApproximateTimeSynchronizer([self.depth_image_sub, self.rgb_image_sub],queue_size=10,slop=0.5)
    self.TimeSynchronizer.registerCallback(self.Detect)

    self.pub_ColorInfo = self.create_publisher(AprilTagInfo, "PosInfo", 1)
    self.TargetJoint5_pub = self.create_publisher(Int16, "set_joint5", 10)

    self.y = 320 #320
    self.x = 240 #240
    self.cur_x = 320
    self.cur_y = 240

    self.dist = Float32()
    self.pr_time = time.time()
    self.cnt = 0
    self.rgb_bridge = CvBridge()
    self.depth_bridge = CvBridge()

    #color
    self.CX_list = []
    self.CY_list = []
    self.Roi_init = ()
    self.hsv_range = ()
    self.circle = (0, 0, 0)
    self.point_pose = (0, 0, 0)
    self.dyn_update = True
    self.select_flags = False
    self.gTracker_state = False
    self.windows_name = 'frame'
    self.Track_state = 'init'
    self.color = color_detect()
    self.cols, self.rows = 0, 0
    self.Mouse_XY = (0, 0)

    self.hsv_text = "/home/yahboom/dofbot_ultra_ws/src/dofbot_ultra_color/dofbot_ultra_color/colorHSV.text"
    #RDK:self.hsv_text = "/home/sunrise/dofbot_ultra_ws/src/3d/dofbot_ultra_color/dofbot_ultra_color/colorHSV.text"
    if os.path.exists(self.hsv_text): self.roi_hsv_range = read_HSV(self.hsv_text)
        self.cx = 0
        self.cy = 0
        self.circle_r = 0 # Prevent misrecognition of other scattered points
        self.pubPos_flag = False
        self.heigh = 0.0
        self.config = global_config
        self.CurEndPos = [-0.006,0.116261662208,0.0911289015753,-1.04719,-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.get_current_end_pos()

        self.declare_parameter('height_scale', 1.0)
        self.declare_parameter('height_offset_mm', 0.0)
        self.height_scale = float(self.get_parameter('height_scale').get_parameter_value().double_value)
        self.height_offset_mm = float(self.get_parameter('height_offset_mm').get_parameter_value().double_value)

        print("Target shape: ",self.color.target_shape)

Let's mainly look at the image processing function Color_Detect,

xxxxxxxxxx
def Color_Detect(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,'bgr8')
    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])
    frame = cv.resize(depth_image, (640, 480))
    depth_image_info = frame.astype(np.float32)
    action = cv.waitKey(10) & 0xFF
    result_image = cv.resize(result_image, (640, 480))
    # Pass the obtained color image as a parameter to process, and simultaneously pass the keyboard event action
    result_frame, binary = self.process(result_image,action)
    print("self.CX_list: ",self.CX_list)
    print("self.CY_list: ",self.CY_list)
    # Traverse self.CX_list
    for i in range(len(self.CX_list)):
        cx = self.CX_list[i]
        cy = self.CY_list[i]
        # Calculate the depth value corresponding to the center coordinates
        dist = depth_image_info[cy,cx]/1000
        # Pass the obtained color block center value coordinates and the depth information corresponding to the center point into the compute_heigh function, calculate the color block height value and perform rounding, convert the unit from meters to millimeters
        heigh = round(self.compute_heigh(cx,cy,dist),2) *1000
        heigh_msg = str(heigh) + 'mm'
        # Call opencv's putText function to draw the height information on the color image
        cv.putText(result_frame, heigh_msg, (cx+5, cy+5), cv.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 2) 
        # Create color block information message and assign values
        pos = AprilTagInfo()
        pos.x = cx
        pos.y = cy
        pos.z = dist
        # Determine whether self.pubPos_flag is 0 and the center corresponding to the color block center is not 0
        if self.pubPos_flag == True and pos.z!=0:
            if i==len(self.CX_list) :
                self.pubPos_flag = False
            # If the calculated value is greater than 50, that is, the height is higher than 5 centimeters, publish the color block information topic message
            if heigh>50:
                self.pub_ColorInfo.publish(pos)
                self.pubPos_flag = False
    if len(binary) != 0: cv.imshow(self.windows_name, ManyImgs(1, ([result_frame, binary])))
    else:
        cv.imshow(self.windows_name, result_frame)

Image processing function self.process,

xxxxxxxxxx
def process(self, rgb_img, action):
    rgb_img = cv.resize(rgb_img, (640, 480))
    binary = []
    # Determine key event, when space is pressed, change the information publishing flag status, self.pubPos_flag being True means information topic can be published
    if action == 32: self.pubPos_flag = True
    # Determine key event, when i or I is pressed, change state to recognition mode
    elif action == ord('i') or action == ord('I'): self.Track_state = "identify"
    # Determine key event, when r or R is pressed, reset all parameters and enter color selection mode
    elif action == ord('r') or action == ord('R'): self.Reset()
    # Determine state value, if it is init, it means initial state value, at this time mouse can be used to select region
    if self.Track_state == 'init':
        cv.namedWindow(self.windows_name, cv.WINDOW_AUTOSIZE)
        # Select a region color within the specified window
        cv.setMouseCallback(self.windows_name, self.onMouse, 0)
        # Determine color selection flag, true means color can be selected
        if self.select_flags == True:
            cv.line(rgb_img, self.cols, self.rows, (255, 0, 0), 2)
            cv.rectangle(rgb_img, self.cols, self.rows, (0, 255, 0), 2)
            # Determine whether the selected region exists
            if self.Roi_init[0] != self.Roi_init[2] and self.Roi_init[1] != self.Roi_init[3]:
                # Call the Roi_hsv function in the created color detection object self.color, returns the processed color image and HSV values
                rgb_img, self.hsv_range = self.color.Roi_hsv(rgb_img, self.Roi_init)
                self.gTracker_state = True
                self.dyn_update = True
            else: self.Track_state = 'init'
    # Determine state value, if it is "identify", it means color recognition can be performed
    elif self.Track_state == "identify":
        # Determine whether HSV threshold file exists, if so read the values inside and assign to hsv_range
        if os.path.exists(self.hsv_text): self.hsv_range = read_HSV(self.hsv_text)
        # Otherwise change state to init for color selection
        else: self.Track_state = 'init'
    if self.Track_state != 'init':
        # Determine the length of self.hsv_range value, that is, determine whether this value exists, when the length is not 0, enter color detection function
        if len(self.hsv_range) != 0:
            rgb_img, binary, self.CX_list,self.CY_list = self.color.ShapeRecognition(rgb_img, self.hsv_range)
            # Determine dynamic parameter update flag, True means the hsv_text file can be updated and values on the parameter server can be modified
            if self.dyn_update == True:
                write_HSV(self.hsv_text, self.hsv_range)
                params = {'Hmin': self.hsv_range[0][0], 'Hmax': self.hsv_range[1][0],
                          'Smin': self.hsv_range[0][1], 'Smax': self.hsv_range[1][1],
                          'Vmin': self.hsv_range[0][2], 'Vmax': self.hsv_range[1][2]}
                self.dyn_client.update_configuration(params)
                self.dyn_update = False
    return rgb_img, binary

4.2. grasp.py

Please refer to section 4.2 【grasp.py】 in the tutorial 【3D Sorting and Grasping Course - 1. AprilTag ID sorting】.