Welcome to DOFBOT-Ultra repository

Depth Camera Ranging

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 starts, clicking on a point in the pseudo-color image will display the distance between that point and the depth camera within the valid data range of the depth camera.

2. Startup and Operation

2.1 Startup

Enter in the terminal:

xxxxxxxxxx
# Start the camera
ros2 launch orbbec_camera dabai_dcw2.launch.py
# Start the ranging program
ros2 run dofbot_ultra_depth get_center_dis

After the program starts, the default is to get the distance of the image center point. If this distance is not within the valid range of the depth camera, an error may be reported. Normal startup is as shown below:

The distance of the point will be displayed in the image, in millimeters (mm), and will also be shown in the terminal.

2.2 Operation

Click any point within the pseudo-color image frame with the mouse. After clicking, a black dot will appear indicating the location of the selected point; the distance of the selected point will be displayed in the image, and the terminal will also display it simultaneously:

In this demonstration, I selected a point on my hand, with a distance of 47.3 cm, which is 473 mm.

3. Program Flowchart

4. Core Code Analysis

The code path is:

xxxxxxxxxx
Raspberry Pi: /home/yahboom/dofbot_ultra_ws/src/dofbot_ultra_depth/dofbot_ultra_depth/get_center_dis.py
RDK: /home/yahboom/dofbot_ultra_ws/src/3d/dofbot_ultra_depth/dofbot_ultra_depth/get_center_dis.py

Import libraries:

xxxxxxxxxx
#ros2py library
import rclpy
from rclpy.node import Node
#opencv image processing library
import cv2 as cv
#cv_bridge library, used for conversion between message data and images
from cv_bridge import CvBridge
#Import image message data type
from sensor_msgs.msg import Image
#Import numpy library, for image data conversion
import numpy as np

Define image encoding format:

xxxxxxxxxx
encoding = ['16UC1', '32FC1']

Initialize data:

xxxxxxxxxx
#define a subscriber to subscribe to depth image topic messages
self.sub = rospy.Subscriber("/camera/depth/image_raw", Image, self.topic)
#define the name of the image window, to facilitate the program to identify mouse click events later
self.window_name = "depth_image"
#initialize x and y point coordinates to the center point, which is (320,240)
self.y = 240
self.x = 320
#create a CVBridge object, to facilitate conversion between depth image topic message data and image data
self.depth_bridge = CvBridge()

Define click_callback function, used to handle detected mouse events:

xxxxxxxxxx
def click_callback(self, event, x, y, flags, params):
    '''event equals 1 means the left mouse button is pressed,
    cv.EVENT_MOUSEMOVE: Mouse move event, value is 0
    cv.EVENT_LBUTTONDOWN: Left button press event, value is 1
    cv.EVENT_LBUTTONUP: Left button release event, value is 2
    cv.EVENT_RBUTTONDOWN: Right button press event, value is 3
    cv.EVENT_RBUTTONUP: Right button release event, value is 4
    cv.EVENT_MBUTTONDOWN: Middle button press event, value is 5
    cv.EVENT_MBUTTONUP: Middle button release event, value is 6
    cv.EVENT_LBUTTONDBLCLK: Left button double-click event, value is 7
    cv.EVENT_RBUTTONDBLCLK: Right button double-click event, value is 8
    cv.EVENT_MBUTTONDBLCLK: Middle button double-click event, value is 9'''
    if event == 1:
        #After pressing the left button, x and y will be returned, update them to the previously defined self.x and self.y
        self.x = x
        self.y = y
        print(self.x)
        print(self.y)

Write the subscriber's callback function, handle received depth image topic data:

xxxxxxxxxx
def topic(self,msg):
    #Use the imgmsg_to_cv2 method of the created depth_bridge to convert received message msg data into image data. The input parameters are the received msg data and the image encoding format, here the value is '32FC1'
    depth_image = self.depth_bridge.imgmsg_to_cv2(msg, encoding[1])
    #Set the image resolution to 640*480
    frame = cv.resize(depth_image, (640, 480))
    #Data conversion, convert the original uint8 data to float32 type to avoid overflow and facilitate obtaining distance information later
    depth_image_info = frame.astype(np.float32)
    #Get the distance of the point based on the x and y values
    dist = depth_image_info[self.y,self.x]
    #Convert the depth image to a pseudo-color image
    depth_image_orin = cv.applyColorMap(cv.convertScaleAbs(depth_image, alpha=0.03),    cv.COLORMAP_JET)
    print("dist: ",dist)
    #Round to 3 decimal places
    dist = round(dist,3)
    #Convert the distance information from number to string and combine it with the unit 'mm'
    dist = 'dist: ' + str(dist) + 'mm'
    #Use opencv's putText function to print the distance information onto the image
    cv.putText(depth_image_orin, dist,  (10, 40), cv.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 255), 2)
    #Detect mouse events within the self.window_name window, enter click_callback callback function to handle mouse events
    cv.setMouseCallback(self.window_name, self.click_callback)
    #Use opencv's circle function to circle the mouse-clicked point on the image
    cv.circle(depth_image_orin,(self.x,self.y),1,(0,0,0),10)
    #Display image
    cv.imshow(self.window_name, depth_image_orin)
    cv.waitKey(1)