Welcome to DOFBOT-Ultra repository

Color Block Volume Measurement

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.

1. Function Description

After the program runs, it recognizes the shape of the selected color block and obtains the relevant data of the color block to calculate the volume of the color block.

2. Startup and Operation

2.1 Startup Commands

Open four terminals and enter the following commands to start:

xxxxxxxxxx
# Start the camera
ros2 launch orbbec_camera dabai_dcw2.launch.py
# Start the low-level control arm driver
ros2 run dofbot_ultra_driver arm_driver
# Start the inverse kinematics program
ros2 run dofbot_ultra_info kinematics_dofbotultra
# Start the volume test program
ros2 run dofbot_ultra_driver calculate_volume

2.2 Operation

After the program starts, place the color block in the image. Use the mouse to select an area within the color block to obtain the HSV value. If the HSV value of the obtained color cannot correctly identify the shape of the color block, you may need to fine-tune the HSV value. Enter the following command in the terminal to start the dynamic parameter adjuster:

xxxxxxxxxx
ros2 run rqt_reconfigure rqt_reconfigure

You can modify the HSV value through the sliders:

The program filters out other colors through the HSV value, keeping only the colors within the HSV value range. The program recognizes the shape of the color block and calculates the color block's data based on the center point of the color block, and then calculates the volume of the color block based on the recognized shape.

Adjusting offset:

Sometimes errors may occur. We can try to fine-tune by setting the offset value:

xxxxxxxxxx
# If you find that moving the wood block position causes large changes in Height, adjust the pitch_bias parameter
ros2 param set /color_detect pitch_bias 0.02

# If you find that Height changes are small but there is a stable deviation from the actual result, adjust the height_bias parameter
ros2 param set /color_detect height_bias 1.2

3. Program Flowchart

calculate_volume.py

4. Core Code Analysis

Code path:

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

Import necessary libraries:

xxxxxxxxxx
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import cv2 as cv
import rclpy
from rclpy.node import Node
import numpy as np
from sensor_msgs.msg import Image
from message_filters import ApproximateTimeSynchronizer, Subscriber
from std_msgs.msg import Float32, Bool
import os
from cv_bridge import CvBridge
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
from rcl_interfaces.msg import ParameterDescriptor
#Import custom message interface
from dofbot_ultra_interface.msg import *  
from dofbot_ultra_interface.srv import *
#Import custom image processing library
from dofbot_ultra_driver.astra_common import *
#Import transformations for processing and calculating transformations in 3D space, including conversion between quaternions and Euler angles
import tf_transformations as tf
#Import transforms3d library for processing transformations in 3D space, performing conversions between quaternions, rotation matrices, and Euler angles, supporting 3D geometric operations and coordinate transformations
import transforms3d as tfs
encoding = ['16UC1', '32FC1']

Program initialization, create publishers, subscribers, etc.:

xxxxxxxxxx
def __init__(self):
    super().__init__('color_detect')
    #Load color HSV configuration file
    self.declare_param()
    #Arm recognizes color block pose
    self.init_joints = [90.0, 120, 0.0, 0.0, 90, 90]
    #Create client to call inverse kinematics service
    self.client = self.create_client(Kinematics, "dofbot_kinematics")
    #Create two subscribers, subscribe to color image topic and depth image topic 
    self.depth_image_sub = Subscriber(self, Image, '/camera/depth/image_raw')
    self.rgb_image_sub = Subscriber(self, Image, '/camera/color/image_raw')
    #Synchronize color and depth image subscription messages by time
    self.ts = ApproximateTimeSynchronizer([self.rgb_image_sub, self.depth_image_sub],queue_size=10,slop=0.5)
    #Process synchronized messages with callback function ComputeVolume, connect callback function with subscribed messages so it is automatically called when new messages are received
    self.ts.registerCallback(self.ComputeVolume)
    #Create bridge for color and depth image topic message data to image data conversion
    self.rgb_bridge = CvBridge()
    self.depth_bridge = CvBridge()
    #Initialize region coordinates
    self.Roi_init = ()
    #Initialize HSV values
    self.hsv_range = ()
    #Initialize recognized color block information: center x coordinate, center y coordinate, and minimum circumscribed circle radius r
    self.circle = (0, 0, 0)
    #Dynamic parameter adjustment flag, when True, dynamic parameter adjustment is performed
    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 coordinates row and column coordinates
    self.cols, self.rows = 0, 0
    #Initialize mouse-selected x and y coordinates
    self.Mouse_XY = (0, 0)
    #Default HSV threshold file path, this file stores the last saved HSV values
    self.hsv_text = "/home/yahboom/dofbot_ultra_ws/src/dofbot_ultra_driver/dofbot_ultra_driver/colorHSV.text"
   
    #Current recognized color block center x and y coordinates and the radius of the color block's minimum circumscribed circle
    self.cx = 0
    self.cy = 0
    self.error = False
    self.circle_r = 0 #Prevent false recognition of other scattered points
    #End effector position pose under the current arm pose
    self.CurEndPos = [0.0, 0.0, 0.0, 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 the relative position and pose between them
    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 the end effector pose under the current arm pose
    self.get_current_end_pos()
    #Initialize corner point coordinate list
    self.get_corner = [0,0,0,0,0,0]

Image processing function ComputeVolume:

xxxxxxxxxx
def ComputeVolume(self,color_frame,depth_frame):
    #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)
    #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 to process as a parameter, along with the keyboard event action
    result_frame, binary= self.process(result_image,action)
    #Check if the current color block's center point coordinates are not 0. If true, a color block has been detected
    if self.color.shape_cx!=0 and self.color.shape_cy!=0:
        #Check the shape of the color block. If it is a cuboid or cylinder, calculate the corner point coordinates
        if self.color.shape_name == "Rectangle" or self.color.shape_name == "Cylinder":
            x1 = self.get_corner[0]
            y1 = self.get_corner[1]
            z1 = depth_image_info[y1,x1]/1000
            if z1!=0:
                c1_pose_T = self.get_pos(x1,y1,z1)
            else:
                self.error = True
                print("z1 invalid Distance!")

            x2 = self.get_corner[2]
            y2 = self.get_corner[3]
            z2 = depth_image_info[y2,x2]/1000
            if z1!=0:
                c2_pose_T = self.get_pos(x2,y2,z2)
            else:
                self.error = True
                print("z2 invalid Distance!")


            x3 = self.get_corner[4]
            y3 = self.get_corner[5]
            z3 = depth_image_info[y3,x3]/1000
            if z1!=0:
                c3_pose_T = self.get_pos(x3,y3,z3)
            else:
                self.error = True
                print("z3 invalid Distance!") 

            cx = self.color.shape_cx
            cy = self.color.shape_cy
            cz = depth_image_info[cy,cx]/1000
            if cz!=0:
                center_pose_T = self.get_pos(cx,cy,cz)
                #Calculate color block height
                height = center_pose_T[2]*100
                print("get height: ",height)

            if self.error!=True:
                # Define coordinates of two points
                point1 = np.array([c1_pose_T[0], c1_pose_T[1], c1_pose_T[2]])
                point2 = np.array([c2_pose_T[0], c2_pose_T[1], c2_pose_T[2]])
                point3 = np.array([c3_pose_T[0], c3_pose_T[1], c3_pose_T[2]])
                c_ponit = np.array([center_pose_T[0], center_pose_T[1], center_pose_T[2]])
                #Calculate radius
                r = np.linalg.norm(point1 - c_ponit)
                # Calculate Euclidean distance (rectangle side length)
                distance1 = np.linalg.norm(point1 - point3)
                distance2 = np.linalg.norm(point3 - point2)

                #Volume of a cuboid equals length times width times height
                if self.color.shape_name == "Rectangle":
                    print("shape_name: ",self.color.shape_name)
                    print("distance1: ",distance1)
                    print("distance2: ",distance2)
                    print("height: ",height)
                    volume = distance1 * distance2 * height
                    print("volume: ",format(volume, 'f'))
                    print("---------------------------")
                 #Volume of a cylinder equals pi times radius squared times height
                if self.color.shape_name == "Cylinder":
                    print("r: ",r)
                    print("height: ",height)
                    print("shape_name: ",self.color.shape_name)
                    volume = math.pi*r*r* height
                    print("volume: ",format(volume, 'f'))
                    print("---------------------------")
        
        if self.color.shape_name == "Square":
            print("shape_name: ",self.color.shape_name)
            cx = self.color.shape_cx
            cy = self.color.shape_cy
            cz = depth_image_info[cy,cx]/1000
            if cz!=0:
                center_pose_T = self.get_pos(cx,cy,cz)
                height = center_pose_T[2]*100
                print("height: ",height)
                #Volume of a cube equals the cube of any side length
                volume = height * height * height
                print("volume: ",format(volume, 'f'))
                print("---------------------------") 

        self.error = False
    if len(binary) != 0: cv.imshow(self.windows_name, ManyImgs(1, ([result_frame, binary])))
    else:
        cv.imshow(self.windows_name, result_frame)

Get position in world coordinate system function get_pos:

xxxxxxxxxx
def get_pos(self,x,y,z):
    #Perform coordinate system transformation to finally get the position pose_T and pose_R of the point in the world coordinate system
    camera_location = self.pixel_to_camera_depth((x,y),z)
    PoseEndMat = np.matmul(self.EndToCamMat, self.xyz_euler_to_mat(camera_location, (0, 0, 0)))
    EndPointMat = self.get_end_point_mat()
    WorldPose = np.matmul(EndPointMat, PoseEndMat) 
    pose_T, pose_R = self.mat_to_xyz_euler(WorldPose)
    return pose_T