Welcome to DOFBOT-Ultra repository

Block Shape 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. If you need to start the web control program backend process again later, you can execute the following in the terminal:

Raspberry Pi:bash ~/dofbot_ultra/APP_dofbot_ultra/start_app.sh
RDK:bash ~/dofbot_ultra_ws/src/app/APP_dofbot_ultra/start_app.sh

1. Function Description

After the program runs, place blocks of the same color. When the camera captures an image, it will perform color recognition based on HSV values and recognize the shape of the color blocks. After pressing the space bar, the robotic arm will grasp the color block of the target shape and place it at a designated location. After placement, it will return to the initial posture for recognizing color blocks.

The shape recognition and grasping logic in this tutorial will automatically switch 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

• Regular USB camera: use usb_cam.launch.py

Both modes retain the functional intent of "grasping after recognizing the target shape":

• DaBai depth camera: Calculate grasping point based on depth chain;

• Regular USB camera: Calculate grasping point based on plane calibration results.

2. Startup and Operation

(1). Startup Commands

Enter the following commands in the terminal to start:

​x
# DaBai depth camera
ros2 launch orbbec_camera dabai_dcw2.launch.py

# Regular USB camera
ros2 launch dofbot_ultra_info usb_cam.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 robotic arm grasping program:
ros2 run dofbot_ultra_driver grasp 
# Start color block shape recognition program:
ros2 run dofbot_ultra_color shape_recognize
# 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}'

Notes:

1. The camera startup command must be selected according to the actual camera type.

2. Except for the camera startup command, the startup methods for other nodes remain consistent.

(2). Operation Flow

After starting the terminal, place blocks of the same color. When the camera captures an image, press [r] or [R] to 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.

xxxxxxxxxx
ros2 run rqt_reconfigure rqt_reconfigure

You can modify the HSV values using sliders. 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 block of the set target shape. The default grasping target shape is Square.

The grasping coordinate calculation method will automatically switch based on the current CAMERA_TYPE:

• Depth camera mode: Calculate grasping point based on depth chain;

• Regular USB camera mode: Calculate grasping point based on plane calibration results.

Modification method:

Modify [shape_recognize.py] line 66:

self.color.target_shape = "Square" # Change to "Cylinder" / "Pentagon" / "Hexagon" / "Heptagon"

3. Program Flowchart

shape_recognize.py

4. Core Code Analysis

4.1. shape_recognize.py

Code path:

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

astra_common code library path:

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

Initialize program parameters, create publishers, subscribers, etc.,

xxxxxxxxxx
def __init__(self):
    super().__init__('shape_detect')
    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]
    # Create publisher for color block information
    self.pub_ColorInfo = self.create_publisher(AprilTagInfo, "PosInfo", 1)
    # Create publisher for robotic arm target angles
    self.pubPoint = self.create_publisher(ArmJoint, "TargetAngle", 1)
    # Create subscriber for gesture recognition results
    self.grasp_status_sub = self.create_subscription(Bool, 'grasp_done', self.GraspStatusCallback, 1)
    # Create two subscribers, subscribing to color image topic and depth image topic
    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.TrackAndGrap)
    # Create bridge for converting color and depth image topic message data to image data
    self.rgb_bridge = CvBridge()
    self.depth_bridge = CvBridge()

    #color
    # Initialize region coordinates
    self.Roi_init = ()
    # Initialize HSV values
    self.hsv_range = ()
    # Initialize recognized color block information, representing color block center x coordinate, center y coordinate, and minimum enclosing circle radius r
    self.circle = (0, 0, 0)
    # Dynamic parameter adjustment flag, True means perform dynamic parameter adjustment
    self.dyn_update = True
    # Mouse selection flag
    self.select_flags = False
    self.gTracker_state = False
    self.windows_name = 'frame'
    self.Track_state = 'init'
    # Create color detection object
    self.color = color_detect()
    # Initialize row and column coordinates of region coordinates
    self.cols, self.rows = 0, 0
    # Initialize mouse-selected xy coordinates
    self.Mouse_XY = (0, 0)
    # Default HSV threshold file path, this file stores the previously saved HSV values
    self.hsv_text = "/home/yahboom/dofbot_ultra_ws/src/dofbot_ultra_color/dofbot_ultra_color/colorHSV.text"
    if os.path.exists(self.hsv_text): self.roi_hsv_range = read_HSV(self.hsv_text)
    # Flag for publishing AprilTag information, publish /PosInfo topic data when True
    self.pubPos_flag = False
    # Set target shape, default is cube, can choose cuboid and cylinder
    self.color.target_shape = "Square" # "Rectangle" ,"Cylinder"
    print("Target shape: ",self.color.target_shape)

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

xxxxxxxxxx
def TrackAndGrap(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)
    # Determine that self.color.shape_cx and self.color.shape_cy are not 0, indicating there are color blocks meeting the conditions
    if self.color.shape_cx!=0 and self.color.shape_cy!=0:
        pos = AprilTagInfo()
        pos.x = self.color.shape_cx
        pos.y = self.color.shape_cy
        print(self.color.shape_cx,self.color.shape_cy)
        pos.z = depth_image_info[self.color.shape_cy,self.color.shape_cx]/1000
        print("depth: ",pos.z)
        # Determine whether the value of self.pubPos_flag is True and the depth value of the color block is not 0, meeting both conditions means the message can be published
        if self.pubPos_flag == True and pos.z!=0:
            self.pubPos_flag = False
            self.pub_ColorInfo.publish(pos)
    # Determine whether the binary image exists, if so display both color and binary images, otherwise only display the color image
    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:
            # Call the ShapeRecognition function in the created color detection object self.color, pass in the color image and self.hsv_range (i.e., HSV threshold), returns the processed color image, binary image, and information of shapes meeting the HSV threshold, including center point coordinates and its minimum enclosing circle radius
            rgb_img, binary, self.circle = 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 the content in section 4.2 [grasp.py] of the tutorial [3D Space Sorting and Grasping \ 1. AprilTag ID Sorting].