Welcome to DOFBOT-Ultra repository

Block Shape 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, place blocks of the same color in the image. After selecting the color, the program calculates the height of each block. When the Space key is pressed, the robotic arm picks up blocks whose height exceeds the set threshold and places them at the designated position. After placement, the arm returns to the detection 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:
ros2 run dofbot_ultra_info kinematics_dofbotultra
# Start arm grasping program:
ros2 run dofbot_ultra_driver grasp 
# Start block shape detection and height program:
ros2 run dofbot_ultra_color shape_height

# Depth camera exposure settings
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}'

2.2. Operation

After the program starts, place blocks of the same color in the image. Click and drag with the mouse to select a region on one block to obtain its HSV value. Upon releasing the mouse, a binary image appears on the right side. All blocks in the color image should be visible in the binary image. If some blocks are not displayed, use the dynamic parameter adjuster by running the following command:

xxxxxxxxxx
ros2 run rqt_reconfigure rqt_reconfigure

Slide the sliders to fine-tune the HSV values.

After the HSV values are tuned, click on the color image frame and press the Space key. The robotic arm will pick up blocks whose height exceeds the set threshold and place them at the designated position.

3. Program Flowchart

shape_height.py

4. Core Code Analysis

4.1. shape_height.py

Code path:

xxxxxxxxxx
Raspberry Pi:/home/yahboom/dofbot_ultra_ws/src/dofbot_ultra_color/dofbot_ultra_color/shape_height.py
RDK:/home/sunrise/dofbot_ultra_ws/src/3d/dofbot_ultra_color/dofbot_ultra_color/shape_height.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.srv import *
from dofbot_ultra_interface.msg import *

Program initialization, creating publishers, subscribers, and clients

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.pr_time = time.time()

    self.init_joints = [90.0, 120.0, 0.0, 0.0, 90.0, 90.0]


    self.pubPoint = self.create_publisher(ArmJoint, "TargetAngle", 1)
    self.grasp_status_sub = self.create_subscription(Bool, 'grasp_done', self.GraspStatusCallback, 1)
    self.pub_ColorInfo = self.create_publisher(AprilTagInfo, "PosInfo", 1)

     # ROS2 subscribers (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.Color_Detect)

    self.client = self.create_client(Kinematics, 'dofbot_kinematics')
    # Create bridges for converting color and depth image topic messages to image data
    self.rgb_bridge = CvBridge()
    self.depth_bridge = CvBridge()
    # Lists storing center x and y values of detected blocks
    self.CX_list = []
    self.CY_list = []
    # Initialize region coordinates
    self.Roi_init = ()
    # Initialize HSV values
    self.hsv_range = ()
    # Dynamic parameter adjustment flag; when True, dynamic parameter adjustment is active
    self.dyn_update = True
    # Mouse selection flag
    self.select_flags = False
    self.gTracker_state = False
    self.windows_name = 'frame'
    # Initialize state value
    self.Track_state = 'init'
    # Create color detection object
    self.color = color_detect()
    # Initialize region row and column coordinates
    self.cols, self.rows = 0, 0
    # Initialize mouse selection xy coordinates
    self.Mouse_XY = (0, 0)
    # Default HSV threshold file path, stores previously saved HSV values
    self.hsv_text = rospkg.RosPack().get_path("dofbot_ultra_color") + "/scripts/colorHSV.text"
    # Load color HSV configuration file, configure dynamic parameter adjuster
    Server(ColorHSVConfig, self.dynamic_reconfigure_callback)
    self.dyn_client = Client(nodeName, timeout=60)
    exit_code = os.system('rosservice call /camera/set_color_exposure  50')
    # Flag for publishing block info; True means can publish block information
    self.pubPos_flag = False
    # Initialize block height value
    self.heigh = 0.0
    # Current arm end-effector position and orientation
    self.CurEndPos = [0,0,0,0,0,0]
    # 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
    self.get_current_end_pos()
    print("Target shape: ",self.color.target_shape)
    # Initialize target block shape
    self.color.target_shape = "Square" # "Rectangle" ,"Cylinder"

Focus on 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 the process function, along with 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)
    # Iterate through self.CX_list
    for i in range(len(self.CX_list)):
        cx = self.CX_list[i]
        cy = self.CY_list[i]
        # Compute depth value at the center coordinates
        dist = depth_image_info[cy,cx]/1000
        # Pass the block center coordinates and corresponding depth information to compute_heigh to compute block height, then scale and keep in meters
        heigh = round(self.compute_heigh(cx,cy,dist),3)
        heigh_msg = str(heigh) + 'm'
        # Draw the height information on the color image using OpenCV's putText function
        cv.putText(result_frame, heigh_msg, (cx+5, cy+5), cv.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 2) 
        # Create and assign block info message
        pos = AprilTagInfo()
        pos.x = cx
        pos.y = cy
        pos.z = dist
        # Check if self.pubPos_flag is True and the depth at the 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 block height exceeds 3 cm, publish the block topic message data
            if heigh>0.03:
                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)

4.2. grasp.py

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