6. ROS2+OpenCV Course

6.1 Color Threshold Adjustment

The color of an object can change with the light sources, which can affect the functionality involved color recognition. To tackle this issue, this lesson will introduce you how to use LAB Tool to adjust the color threshold.

6.1.1 Open/Close LAB TOOL

Note

• The entered command should be case sensitive, and the “Tab” key can be used to complement key words.

• Please strictly follow the operation steps; otherwise, you will fail to open LAB tool.

(1) Start the robot and connect it to VNC remote connection software.

(2) Click on to start the command line terminal.

(3) Input the command and press Enter to disable app auto-start service.

~/.stop_ros.sh

(4) Input the command and press Enter to enable the camera node.

ros2 launch peripherals depth_camera.launch.py

(5) Open a new command-line terminal and input the command to open LAB tool.

python3 /home/ubuntu/software/lab_tool/main.py

(6) Regarding the interface button instructions and usage, you can refer to the following content. If you want to close it, please click on Quit button.

(7) After closing the LAB TOOL, click to navigate to the command-line terminal. Then enter a command to restart the app’s auto-start service. Once the service is restarted, the robotic arm will return to its initial position.

sudo systemctl restart start_app_node.service

Note

If you do not enable app auto-start service for the robot, it will affect the corresponding app functions. Restarting robot can also automatically enable the app auto-start service.

6.1.2 LAB TOOL Interface Layout Instruction

The interface of LAB tool software is divided into two parts: image display area and recognition adjustment area.

(1) Image display area: the left displays the processed image and the right is the raw image.

Note

If the transmitted image does not display normally, it’s the issue of camera connection. In this case, you need to examine if the wiring is connected properly or reconnect it.

(2) Recognition adjustment area: adjust the color threshold. The function of each button refer to the following table:

Icon	Function instruction
	The sliders L, A and B are used respectively to adjust the values of the corresponding L, A and B components of the image. The sliders on the left are the "min" value for each component while the slider on the right are the "max" value for each component.
	Choose color to adjust the threshold.
	Delete the currently selected color.
	Add recognizable color.
	Save the adjustment result of the color threshold.
	Click on this button to swap Depth camera/monocular camera.
	Close LAB TOOL.

6.1.3 Adjust Color Threshold

(1) Open LAB tool. Choose the color in the color drop-down list of color recognition area. Take a color “red” as example.

(2) Adjust the “min” values of L, A and B components to “0”, the “max” values to “225”.

(3) Place the target within the camera’s field of view. Adjust the values of L, A and B component towards the interval representing the target recognition color according to the LAB color space distribution chart.

Red color is close to “+a”, which indicates that A component needs to be increased. Therefore, keep the “max” value of the A component unchanged and increase its “min” value until the object on the left turns white while other areas turn black.

(4) Adjust the L and B components according to your surroundings. If the red color is light, increase the “min” value of the L component; if it is dark, decrease the “max” value of the component. If the red color tends to be warm, increase the “min” value of the B component; if it tends to be cool, decrease the “max” value of the B component.

The following table shows the parameter information of LAB threshold adjustment:

Color component	Value Range	Corresponding Color Intervals
L	0~255	Black-white (-L~+L)
A	0~255	green-red（-a ~ +a）
B	0~255	Blue-yellow（-b ~ +b）

(5) Click “Save” button in adjustment area to save the adjustment parameters.

6.1.4 Add New Color

In addition to the built-in colors, users can add other recognition colors. Take an example of adding “yellow”.

(1) Open LAB tool and click on “Add” button.

(2) Fill in the “name” column with the color name, and click the “OK” button.

(3) Choose the added color in the color drop-down menu.

(4) Place the target within the camera’s field of view, and adjust the threshold by dragging the L, A, and B component sliders until the area of the colored object within the left screen turns white, while the other areas turn black.

Note

the adjustment method of color threshold refers to “6.1.3 Adjust Color Adjustment”.

(5) Click “Save” button in recognition adjustment area to save the adjustment parameters of color threshold.

After adjusting the color thresholds, press ‘Ctrl+C’ to stop the camera service, then click ‘Close Software’ to close the interface.

6.2 Color Recognition

This session will use OpenCV to perform red, green, blue recognition and display the recognition result through transmitted image. Before operations, please prepare one object of red, green and blue.

6.2.1 Recognition Process

Firstly, obtain the RGB image from the camera. Scale and apply Gaussian blur to the image. Convert the image from RGB to Lab. (For detailed explanations regarding the Lab color space, you can refer to “OpenCV Basic Lesson”.)

Next, identify the object color in the circle by using color thresholds, followed by masking of the image portion. (masking is used to hide certain part of the processed image).

After performing opening and closing operations on the object image, the object with the largest contour is circled.

Open operations: erosion followed by dilation. Remove small objects, smooth the contours of object and keep its area unchanged. It can eliminate small noise and break thin connection between objects.

Erosion: it can be used to remove small objects or features from an image, break thin connections between objects, and generally reduce the size of objects.

Dilation: it is useful for filling in gaps between objects, joining nearby objects, and generally increasing the size of objects.

Finally, the recognition results are displayed on the screen.

6.2.2 Operation Steps

Note

The entered command should be case sensitive and “Tab” key can be used to complement the key words.

(1) Start JetAuto and connect it to VNC.

(2) Click on to start the command line terminal.

(3) Input the command and press Enter to disable the app auto-start service.

~/.stop_ros.sh

(4) Run the following command to enable the camera node:

ros2 launch peripherals depth_camera.launch.py

(5) Open a new command line terminal, enter the following command to navigate to the program directory and start the color recognition game:

cd ~/ros2_ws/src/example/example/color_detect && python3 color_detect_demo.py

(6) The program will launch the camera’s image interface. For detailed recognition steps, please refer to section 2.3 Program Outcome.

(7) Next, press “Ctrl+C” in the command line terminal interface. If closing fails, please try again repeatedly.

6.2.3 Program Outcome

Note

After the game starts, please ensure that there are no other objects containing recognized colors within the field of view of the camera to avoid affecting the implementation effect.

After the game starts, place the target object within the field of view of the camera. When recognizing the target object, it will be circled up in the same color and the color name will be printed in the lower-left corner of the screen. The program supports red, blue and green object recognition.

6.2.4 Program Analysis

The source code of the program is located in : /home/ubuntu/ros2_ws/src/example/example/color_detect/color_detect_demo.py

• Main Function

Initialize the ROS node, create a color recognition node named “color_detect_node”, subscribe to the image topic “/depth_cam/rgb/image_raw”, and set its callback function to “image_callback”. Start the main function for color recognition using multiple threads. Wait for the node to shut down.

if __name__ == '__main__':
    rclpy.init()
    node = rclpy.create_node('color_detect_node')
    camera_device = 'depth_cam'
    if camera_device == 'usb_cam':
        node.create_subscription(Image, '/usb_cam/image_raw', image_callback, 1)
    else:
        node.create_subscription(Image, '/depth_cam/rgb/image_raw', image_callback, 1)
    threading.Thread(target=main, daemon=True).start()
    rclpy.spin(node)

• Image Processing

(1) Camera Callback Function

Primarily used to read the video stream from the topic and enqueue it to ensure real-time display.

def image_callback(ros_image):
    bgr_image = bridge.imgmsg_to_cv2(ros_image, "bgr8")
    if image_queue.full():
        # 如果队列已满，丢弃最旧的图像
        image_queue.get()
        # 将图像放入队列
    image_queue.put(bgr_image)

(2) Color Recognition Main Function

This function reads image data from the queue, processes it using the run function for color recognition, and then displays the recognized image using cv2.

def main():
    running = True
    while running:
        try:
            image = image_queue.get(block=True, timeout=1)
        except queue.Empty:
            if not running:
                break
            else:
                continue
        image = run(image)
        cv2.imshow('image', image)
        key = cv2.waitKey(1)
        if key == ord('q') or key == 27:  # 按q或者esc退出
            break

(3) YAML File Reading Function

def get_yaml_data(yaml_file):
    yaml_file = open(yaml_file, 'r', encoding='utf-8')
    file_data = yaml_file.read()
    yaml_file.close()
    
    data = yaml.load(file_data, Loader=yaml.FullLoader)
    
    return data

(4) Maximum Contour Function

This function takes contours detected by OpenCV as input, identifies the largest contour based on size, and returns the contour along with its area.

# 找出面积最大的轮廓
# 参数为要比较的轮廓的列表
def getAreaMaxContour(contours):
    contour_area_temp = 0
    contour_area_max = 0
    area_max_contour = None

    for c in contours:  # 历遍所有轮廓
        contour_area_temp = math.fabs(cv2.contourArea(c))  # 计算轮廓面积
        if contour_area_temp > contour_area_max:
            contour_area_max = contour_area_temp
            if contour_area_temp > 50:  # 只有在面积大于50时，最大面积的轮廓才是有效的，以过滤干扰
                area_max_contour = c

    return area_max_contour, contour_area_max  # 返回最大的轮廓

(5) Color Recognition Function

This function compresses the input image to a size of (320, 240) for easier detection, applies Gaussian filtering, and then converts the image to the LAB color space.

def run(img):
    global draw_color
    global color_list
    global detect_color
    
    img_copy = img.copy()
    img_h, img_w = img.shape[:2]

    frame_resize = cv2.resize(img_copy, size, interpolation=cv2.INTER_NEAREST)
    frame_gb = cv2.GaussianBlur(frame_resize, (3, 3), 3)      
    frame_lab = cv2.cvtColor(frame_gb, cv2.COLOR_BGR2LAB)  # 将图像转换到LAB空间

    max_area = 0
    color_area_max = None    
    areaMaxContour_max = 0

    for i in ['red', 'green', 'blue']:
        frame_mask = cv2.inRange(frame_lab, tuple(lab_data['lab']['Stereo'][i]['min']), tuple(lab_data['lab']['Stereo'][i]['max']))  #对原图像和掩模进行位运算
        eroded = cv2.erode(frame_mask, cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3)))  #腐蚀
        dilated = cv2.dilate(eroded, cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3))) #膨胀
        contours = cv2.findContours(dilated, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)[-2]  #找出轮廓
        areaMaxContour, area_max = getAreaMaxContour(contours)  #找出最大轮廓
        if areaMaxContour is not None:
            if area_max > max_area:#找最大面积
                max_area = area_max
                color_area_max = i
                areaMaxContour_max = areaMaxContour

The function iterates through a list of colors to identify the color of the largest detected contour.

    for i in ['red', 'green', 'blue']:
        frame_mask = cv2.inRange(frame_lab, tuple(lab_data['lab']['Stereo'][i]['min']), tuple(lab_data['lab']['Stereo'][i]['max']))  #对原图像和掩模进行位运算
        eroded = cv2.erode(frame_mask, cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3)))  #腐蚀
        dilated = cv2.dilate(eroded, cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3))) #膨胀
        contours = cv2.findContours(dilated, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)[-2]  #找出轮廓
        areaMaxContour, area_max = getAreaMaxContour(contours)  #找出最大轮廓
        if areaMaxContour is not None:
            if area_max > max_area:#找最大面积
                max_area = area_max
                color_area_max = i
                areaMaxContour_max = areaMaxContour

Contours with an area smaller than 200 are filtered out. If the area is larger than 200, the function calculates the contour coordinates and draws the detected color’s position on the screen.

    if max_area > 200:  # 有找到最大面积
        ((centerX, centerY), radius) = cv2.minEnclosingCircle(areaMaxContour_max)  # 获取最小外接圆
        centerX = int(common.val_map(centerX, 0, size[0], 0, img_w))
        centerY = int(common.val_map(centerY, 0, size[1], 0, img_h))
        radius = int(common.val_map(radius, 0, size[0], 0, img_w))            
        cv2.circle(img, (centerX, centerY), radius, range_rgb[color_area_max], 2)#画圆

        if color_area_max == 'red':  #红色最大
            color = 1
        elif color_area_max == 'green':  #绿色最大
            color = 2
        elif color_area_max == 'blue':  #蓝色最大
            color = 3
        else:
            color = 0
        color_list.append(color)

        if len(color_list) == 3:  #多次判断
            # 取平均值
            color = int(round(np.mean(np.array(color_list))))
            color_list = []
            if color == 1:
                detect_color = 'red'
                draw_color = range_rgb["red"]
            elif color == 2:
                detect_color = 'green'
                draw_color = range_rgb["green"]
            elif color == 3:
                detect_color = 'blue'
                draw_color = range_rgb["blue"]
            else:
                detect_color = 'None'
                draw_color = range_rgb["black"]               
    else:
        detect_color = 'None'
        draw_color = range_rgb["black"]
            
    cv2.putText(img, "Color: " + detect_color, (10, img.shape[0] - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.65, draw_color, 2)
    
    return img

6.3 Generate & Recognize QR Code

This lesson is divided into two parts. The first part will introduce you how to learn to create a QR code, while the second part focuses on recognizing the created QR code and then decoding the QR code information through the terminal.

6.3.1 Generate QR Code

• Process

First, create an instance object of the QR code tool and set its detailed parameters.

Next, obtain the user’s data and fill it into the QR code.

Finally, generate a QR CODE image based on the data and display it in a window, and save it in the corresponding path.

• Operation Steps

Note

The entered command should be case sensitive and the “Tab” key can be used to fill in key words.

(1) Start the robot and connect it to VNC.

(2) Click on to start the command line terminal.

(3) Input the command and press Enter to disable the app auto-start service.

~/.stop_ros.sh

(4) Input the command to access the program directory and start the QR code creation program.

cd ~/ros2_ws/src/example/example/qrcode && python3 qrcode_creater.py

After start the program, it’s necessary to enter characters in the terminal to generate the QR code. Take the input of “ubuntu” as example.

Press “Enter” to display a QR code image containing the input data.

(5) Next, press the ESC key or q key to exit the QR code interface.

(6) When you return to the command-line terminal and see the prompt shown in the image below, it means the QR code has been successfully saved.

(7) Enter the command to save the file in the tmp folder:

cp myQRcode.jpg /home/ubuntu/share/tmp/

(8) Use the SSH remote connection tool MobaXterm to drag the myQRcode.jpg image from the /home/pi/docker/tmp folder on the Raspberry Pi system to your local PC.

(8) For details on how to connect using the SSH remote connection tool MobaXterm, please refer to “1.Quick Start Guide(JetAuto User Manual )->1.6 Development Environment Setup and Configuration”.

• Program Analysis

The source code of this program is located at: /home/ubuntu/ros2_ws/src/example/example/qrcode/qrcode_creater.py

(1) Creating QR Code Utility Object

Using the qrcode module to create the necessary object and setting various parameters for the QR code.

    qr = qrcode.QRCode(
        version=1,
        error_correction=qrcode.constants.ERROR_CORRECT_H,
        box_size=5,
        border=4)
    # 添加数据
    qr.add_data(data)

The parameters of the above function are as follows:

The first parameter version is an integer ranging from 1 to 40, used to control the size of the QR code. If you want the program to automatically determine the size, set this value to None and use the “fit” parameter.

The second parameter error_correction controls the error correction capability of the QR code, with the following options:

① ERROR_CORRECT_L: can correct approximately 7% or fewer errors.

② ERROR_CORRECT_M: default value, can correct approximately 15% or fewer errors.

③ ERROR_CORRECT_H: can correct approximately 30% or fewer errors.

The third parameter box_size controls the number of pixels contained in each small box of the QR code.

The fourth parameter border controls the number of boxes included in the border (distance between the QR code and the image boundary), with a default value of 4, which is the minimum value specified by relevant standards.

(2) Generating QR Code

Using the add_data and make functions to retrieve and fill data, and then using the make_image function to generate the image.

    # 添加数据
    qr.add_data(data)
    # 填充数据
    qr.make(fit=True)
    # 生成图片
    img = qr.make_image(fill_color=(0, 0, 0), back_color=(255, 255, 255))
    opencv_img = cv2.cvtColor(np.asarray(img), cv2.COLOR_RGB2BGR)

The parameters of the make_image function are as follows:

The first parameter fill_color=(0, 0, 0) is the fill color of the image, which is black in this case.

The second parameter back_color=(255, 255, 255) is the background color of the image, which is white in this case.

(3) Displaying Image

Converting the color space of the image using the cvtColor function, and then displaying it on the window using the imshow function.

    opencv_img = cv2.cvtColor(np.asarray(img), cv2.COLOR_RGB2BGR)
    while True:
        cv2.imshow('img', opencv_img)
        k = cv2.waitKey(1)
        if k != -1:
            break

(4) Saving Image

Using the imwrite function to store the generated QR code image and printing relevant information.

    cv2.imwrite(file_name, opencv_img)
    print('save', data, file_name)

The parameters of the imwrite function are as follows:

The first parameter file_name is the storage path of the image.

The second parameter opencv_img is the image to be stored.

6.3.2 QR Code Recognition

In the previous section, we created a QR code. In this section, we’ll perform content recognition on the QR code.

• Recognition Process

Firstly, create an instance object for QR code detection, and pass in the required network structure and model weight files for detection.

Then, obtain the camera’s feedback image and perform detection.

Finally, once the QR code is recognized, it will be framed, and the content of the QR code will be printed.

The source code for this program is located at: /home/ubuntu/ros2_ws/src/example/example/qrcode/qrcode_detecter.py

• QR Code Recognition Steps

Note

When entering commands, it is necessary to strictly distinguish between uppercase and lowercase letters, and you can use the “Tab” key to complete keywords.

(1) Start the robot and connect it to the remote control software VNC.

(2) Click on on the system desktop to start the command line terminal.

(3) Enter the command to disable the automatic startup service of the app:

~/.stop_ros.sh

(4) Enter the command to start the camera node:

ros2 launch peripherals depth_camera.launch.py

(5) Open a new ROS2 command line terminal, enter the command to switch to the program directory, and start the QR code recognition program:

cd ~/ros2_ws/src/example/example/qrcode && python3 qrcode_detecter.py

To close this game, press “Ctrl+C” in the terminal interface. If closing fails, please try again repeatedly.

• Program Outcome

After starting the game, it will recognize the QR code images appearing in the feedback screen, mark them with a red box, and print out the content of the QR code.

• Program Analysis

The source code of this program is saved in /home/ubuntu/ros2_ws/src/example/example/qrcode/qrcode_detecter.py

(1) Main Function

Initialize the QRCodeDetectNode and set the node name as qrcode_detect. Wait for the ROS node to close, and if it closes, unregister the current node.

def main():
    node = QRCodeDetectNode('qrcode_detect')
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        node.destroy_node()
        rclpy.shutdown()
        print('shutdown')
    finally:
        print('shutdown finish')

if __name__ == "__main__":
    main()

(2) QRCodeDetectNode Class

Initialize the node and define parameters, then start the self.main function.

Parameters:

self.running: Whether the detection is enabled

self.model_path: Model path

self.image_queue: Image queue

self.image_sub: Subscription to read camera feedback images

self.qcd: Initialization of the QR code detection

class QRCodeDetectNode(Node):
    def __init__(self, name):
        rclpy.init()
        super().__init__(name)
        self.running = True
        self.bridge = CvBridge()
        self.model_path = os.path.join(os.path.abspath(os.path.split(os.path.realpath(__file__))[0]), 'model/detector.tflite')
        self.image_queue = queue.Queue(maxsize=2)
        self.image_sub = self.create_subscription(Image, '/depth_cam/rgb/image_raw', self.image_callback, 1)
        self.qcd = cv2.QRCodeDetector()
        threading.Thread(target=self.main, daemon=True).start()  

    def image_callback(self, ros_image):
        cv_image = self.bridge.imgmsg_to_cv2(ros_image, "bgr8")
        bgr_image = np.array(cv_image, dtype=np.uint8)
        if self.image_queue.full():
            # 如果队列已满，丢弃最旧的图像
            self.image_queue.get()
            # 将图像放入队列
        self.image_queue.put(bgr_image)

    def main(self):
        while self.running:
            try:
                image = self.image_queue.get(block=True, timeout=1)
            except queue.Empty:
                if not self.running:
                    break
                else:
                    continue
            decoded_objects = pyzbar.decode(image)
            for obj in decoded_objects:
                points = obj.polygon
                if len(points) > 4:
                    hull = cv2.convexHull(np.array([point for point in points], dtype=np.float32))
                    points = hull.reshape(-1, 2)
                for j in range(len(points)):
                    cv2.line(image, tuple(points[j]), tuple(points[(j+1) % len(points)]), (0, 255, 0), 3)
                x = obj.rect.left
                y = obj.rect.top
                barcode_data = obj.data.decode("utf-8")
                print(barcode_data)
                cv2.putText(image, barcode_data, (x, y - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)

            cv2.imshow('image', image)
            key = cv2.waitKey(1)
            if key == ord('q') or key == 27:  # 按q或者esc退出
                break

Functions:

The camera callback function reads the camera feedback image and places it in the queue self.image_queue for automatic update and discarding of expired images.

    def image_callback(self, ros_image):
        cv_image = self.bridge.imgmsg_to_cv2(ros_image, "bgr8")
        bgr_image = np.array(cv_image, dtype=np.uint8)
        if self.image_queue.full():
            # 如果队列已满，丢弃最旧的图像
            self.image_queue.get()
            # 将图像放入队列
        self.image_queue.put(bgr_image)

The main function begins by performing QR code detection. It checks the self.running parameter to determine whether to start detection. If True, it reads image data from the self.image_queue queue, inputs it into the initialized recognition model self.qcd, and finally prints the recognition results and draws recognition boxes based on the output data.

    def main(self):
        while self.running:
            try:
                image = self.image_queue.get(block=True, timeout=1)
            except queue.Empty:
                if not self.running:
                    break
                else:
                    continue
            decoded_objects = pyzbar.decode(image)
            for obj in decoded_objects:
                points = obj.polygon
                if len(points) > 4:
                    hull = cv2.convexHull(np.array([point for point in points], dtype=np.float32))
                    points = hull.reshape(-1, 2)
                for j in range(len(points)):
                    cv2.line(image, tuple(points[j]), tuple(points[(j+1) % len(points)]), (0, 255, 0), 3)
                x = obj.rect.left
                y = obj.rect.top
                barcode_data = obj.data.decode("utf-8")
                print(barcode_data)
                cv2.putText(image, barcode_data, (x, y - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)

            cv2.imshow('image', image)
            key = cv2.waitKey(1)
            if key == ord('q') or key == 27:  # 按q或者esc退出
                break
        
        cv2.destroyAllWindows()
        rclpy.shutdown()

6.4 Autonomous Line Following

6.4.1 Recognition Procedure

First, obtain the RGB image from the camera and resize it. Apply Gaussian blur to the image and convert the color space from RGB to Lab. (For detailed explanation on Lab color space, please refer to “OpenCV Basics Course”).

Then, perform color recognition on the objects in the circle using color thresholding. Apply masking to parts of the image. (Masking is used to globally or locally mask the processed image using selected images, graphics, or objects).

Afterward, perform opening and closing operations on the object images. Finally, outline the largest object with a circle.

Opening operation: First, erode the image and then dilate it. Purpose: Used to eliminate small objects, smooth the shape boundary, and not change its area. It can remove small grain noise and break the adhesion between objects.

Erosion: Eliminate the boundary points of the object, causing the boundary to shrink inward, and can remove objects smaller than the structural element.

Dilation: Expand the boundary points of the object, merge all background points in contact with the object into the object, and expand the boundary outward.

Finally, feedback the recognition results on the feedback screen.

6.4.2 Autonomous Line Following Operation

Note

The input command should be case sensitive, and keywords can be complemented using Tab key.

(1) Start the robot and connect it to the remote control software VNC.

(2) Click on on the system desktop to start the command line terminal.

(3) Enter the command to disable the automatic startup service of the app:

~/.stop_ros.sh

(4) Run the command to enable the camera node

ros2 launch app line_following_node.launch.py debug:=true

(5) Create a new command line terminal, enter the command to navigate to the program directory, and start the line-following game:

ros2 service call /line_following/enter std_srvs/srv/Trigger {}

(6) Then, in the current command line terminal, enter the command to navigate to the program directory and start the line-following game:

ros2 service call /line_following/set_running std_srvs/srv/SetBool "{data: True}"

(7) Click on the position of the line in the image to select the corresponding color of the track line, and you can start line following by color picking.

(8) If you need to close this function, you need to return to the command line terminal and enter the command to stop it.

ros2 service call /line_following/enter std_srvs/srv/Trigger {}

6.4.3 Program Analysis

• Launch Analysis:

The program file corresponding to this section of the course document is located at:

/home/ubuntu/ros2_ws/src/app/launch/line_following_node.launch.py

(1) Read the Storage Path

    if compiled == 'True':
        controller_package_path = get_package_share_directory('controller')
        peripherals_package_path = get_package_share_directory('peripherals')
    else:
        controller_package_path = '/home/ubuntu/ros2_ws/src/driver/controller'
        peripherals_package_path = '/home/ubuntu/ros2_ws/src/peripherals'

Use get_package_share_directory to obtain the package path.

(2) Launch

    line_following_node = GroupAction([
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                os.path.join(peripherals_package_path, 'launch/lidar.launch.py')),
            condition=IfCondition(debug),
            ),

        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                os.path.join(peripherals_package_path, 'launch/depth_camera.launch.py')),
            condition=IfCondition(debug),
            ),

        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                os.path.join(controller_package_path, 'launch/controller.launch.py')),
            condition=IfCondition(debug),
            ),

        Node(
            package='app',
            executable='line_following',
            output='screen',
            parameters=[{'debug': debug}],
            ),
    ])

controller_launch: Start motion control launch.

depth_camera_launch: Start depth camera launch.

lidar_launch: Start lidar launch.

(3) None

line_following_node: Start line following node.

        Node(
            package='app',
            executable='line_following',
            output='screen',
            parameters=[{'debug': debug}],
            ),

(4) Source Code Analysis:

The program file corresponding to this section of the course document is located at:

/home/ubuntu/ros2_ws/src/app/app/line_following.py

Note

It’s necessary to back up the original program file before making any modification to the program. You’re forbidden to modify the original source code file to avoid causing robot malfunctions that may be irreparable due to incorrect parameter modifications!

According to the game’s effect, the process logic of this game is summarized as shown in the following diagram:

Subscribe to the topic messages published by the camera node to obtain RGB images. From the image, identify and select the target line. Determine the color threshold by picking the color of the line. Based on the line’s color information, extract the features of the line for line following. Calculate the robot’s offset relative to the line’s position in the field of view. Control the robot to move along the line segment, continuously correcting its position to keep the line at the center of the field of view and use lidar to detect obstacles and avoid them.

Then, the color picker object is defined (used in the game to pick colors), along with the setting of additional Lidar obstacle avoidance functionality. Next is the implementation of the LineFollower class, which mainly includes functions for image preprocessing and logic for line following.

Function:

Main:

def main():
    node = LineFollowingNode('line_following')
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

Initiate line following node:

Class:

LineFollower:

class LineFollower:
    def __init__(self, color, node):
        self.node = node

Init:

    def __init__(self, color, node):
        self.node = node
        self.target_lab, self.target_rgb = color
        if os.environ['MACHINE_TYPE'] == 'JetAuto':
            self.rois = ((0.90, 0.92, 0, 1, 0.7), (0.78, 0.80, 0, 1, 0.2), (0.66, 0.68, 0, 1, 0.1))
        else:
            self.rois = ((0.81, 0.83, 0, 1, 0.7), (0.69, 0.71, 0, 1, 0.2), (0.57, 0.59, 0, 1, 0.1))
        self.weight_sum = 1.0

Set the line-following color and ROI list.

get_area_max_contour:

    def get_area_max_contour(contours, threshold=100):
        '''
        获取最大面积对应的轮廓(get the contour of the largest area)
        :param contours:
        :param threshold:
        :return:
        '''
        contour_area = zip(contours, tuple(map(lambda c: math.fabs(cv2.contourArea(c)), contours)))
        contour_area = tuple(filter(lambda c_a: c_a[1] > threshold, contour_area))
        if len(contour_area) > 0:
            max_c_a = max(contour_area, key=lambda c_a: c_a[1])
            return max_c_a
        return None

Get the contour with the maximum area.

Call:

    def __call__(self, image, result_image, threshold):
        centroid_sum = 0
        h, w = image.shape[:2]
        min_color = [int(self.target_lab[0] - 50 * threshold * 2),
                     int(self.target_lab[1] - 50 * threshold),
                     int(self.target_lab[2] - 50 * threshold)]
        max_color = [int(self.target_lab[0] + 50 * threshold * 2),
                     int(self.target_lab[1] + 50 * threshold),
                     int(self.target_lab[2] + 50 * threshold)]
        target_color = self.target_lab, min_color, max_color
        for roi in self.rois:
            blob = image[int(roi[0]*h):int(roi[1]*h), int(roi[2]*w):int(roi[3]*w)]  # 截取roi(intercept roi)
            img_lab = cv2.cvtColor(blob, cv2.COLOR_RGB2LAB)  # rgb转lab(convert rgb into lab)
            img_blur = cv2.GaussianBlur(img_lab, (3, 3), 3)  # 高斯模糊去噪(perform Gaussian filtering to reduce noise)
            mask = cv2.inRange(img_blur, tuple(target_color[1]), tuple(target_color[2]))  # 二值化(image binarization)
            eroded = cv2.erode(mask, cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3)))  # 腐蚀(corrode)
            dilated = cv2.dilate(eroded, cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3)))  # 膨胀(dilate)
            # cv2.imshow('section:{}:{}'.format(roi[0], roi[1]), cv2.cvtColor(dilated, cv2.COLOR_GRAY2BGR))
            contours = cv2.findContours(dilated, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_TC89_L1)[-2]  # 找轮廓(find the contour)
            max_contour_area = self.get_area_max_contour(contours, 30)  # 获取最大面积对应轮廓(get the contour corresponding to the largest contour)
            if max_contour_area is not None:
                rect = cv2.minAreaRect(max_contour_area[0])  # 最小外接矩形(minimum circumscribed rectangle)
                box = np.intp(cv2.boxPoints(rect))  # 四个角(four corners)
                for j in range(4):
                    box[j, 1] = box[j, 1] + int(roi[0]*h)
                cv2.drawContours(result_image, [box], -1, (0, 255, 255), 2)  # 画出四个点组成的矩形(draw the rectangle composed of four points)

Perform color recognition, identify based on the set color, and provide feedback on the recognized image and angle.

LineFollowingNode:

class LineFollowingNode(Node):
    def __init__(self, name):
        rclpy.init()
        super().__init__(name, allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True)
        
        self.name = name
        self.set_callback = False
        self.is_running = False

Init：

    def __init__(self, name):
        rclpy.init()
        super().__init__(name, allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True)
        
        self.name = name
        self.set_callback = False
        self.is_running = False
        self.color_picker = None
        self.follower = None
        self.scan_angle = math.radians(45)
        self.pid = pid.PID(0.01, 0.0, 0.0)
        self.empty = 0
        self.count = 0
        self.stop = False

Initialize the required parameters for the program, call the base node, camera node, and start services such as enter, exit, start, set color, get color, set threshold, etc.

get_node_state:

    def get_node_state(self, request, response):
        response.success = True
        return response

Set the status of the current node.

Main:

    def main(self):
        while True:
            try:
                image = self.image_queue.get(block=True, timeout=1)
            except queue.Empty:
                continue

            result = cv2.cvtColor(image, cv2.COLOR_RGB2BGR)
            cv2.imshow("result", result)
            if self.debug and not self.set_callback:
                self.set_callback = True
                # 设置鼠标点击事件的回调函数
                cv2.setMouseCallback("result", self.mouse_callback)
            k = cv2.waitKey(1)
            if k != -1:
                break

Read the image and use the mouse to pick colors.

mouse_callback:

    def mouse_callback(self, event, x, y, flags, param):
        if event == cv2.EVENT_LBUTTONDOWN:
            self.get_logger().info("x:{} y{}".format(x, y))
            msg = SetPoint.Request()
            if self.image_height is not None and self.image_width is not None:
                msg.data.x = x / self.image_width
                msg.data.y = y / self.image_height
                self.set_target_color_srv_callback(msg, SetPoint.Response())

Mouse color picking callback function, obtains the pixel coordinates of the current mouse position.

enter_srv_callback:

    def enter_srv_callback(self, request, response):
        self.get_logger().info('\033[1;32m%s\033[0m' % "line following enter")
        with self.lock:
            self.stop = False
            self.is_running = False
            self.color_picker = None
            self.pid = pid.PID(1.1, 0.0, 0.0)
            self.follower = None
            self.threshold = 0.5
            self.empty = 0
            if self.image_sub is None:
                if self.machine_type == 'JetAuto':
                    self.image_sub = self.create_subscription(Image, '/depth_cam/rgb/image_raw', self.image_callback, 1)  # 摄像头订阅(subscribe to the camera)
                else:
                    self.image_sub = self.create_subscription(Image, '/usb_cam/image_raw', self.image_callback, 1)  # 摄像头订阅(subscribe to the camera)

Enter autonomous line-following service, read data from the camera and lidar, and initialize actions.

exit_srv_callback:

    def exit_srv_callback(self, request, response):
        self.get_logger().info('\033[1;32m%s\033[0m' % "line following exit")
        try:
            if self.image_sub is not None:
                self.destroy_subscription(self.image_sub)
                self.image_sub = None
            if self.lidar_sub is not None:
                self.destroy_subscription(self.lidar_sub)
                self.lidar_sub = None
        except Exception as e:
            self.get_logger().error(str(e))
        with self.lock:
            self.is_running = False
            self.color_picker = None
            self.pid = pid.PID(0.01, 0.0, 0.0)
            self.follower = None
            self.threshold = 0.5
            self.mecanum_pub.publish(Twist())
        response.success = True
        response.message = "exit"
        return response

Exit autonomous line-following service, close various reading nodes, reset PID, and stop line following.

set_target_color_srv_callback:

    def set_target_color_srv_callback(self, request, response):
        self.get_logger().info('\033[1;32m%s\033[0m' % "set_target_color")
        with self.lock:
            x, y = request.data.x, request.data.y
            self.follower = None
            if x == -1 and y == -1:
                self.color_picker = None
            else:
                self.color_picker = ColorPicker(request.data, 5)
                self.mecanum_pub.publish(Twist())
        response.success = True
        response.message = "set_target_color"
        return response

Set line-following color service.

get_target_color_srv_callback:

    def get_target_color_srv_callback(self, request, response):
        self.get_logger().info('\033[1;32m%s\033[0m' % "get_target_color")
        response.success = False
        response.message = "get_target_color"
        with self.lock:
            if self.follower is not None:
                response.success = True
                rgb = self.follower.target_rgb
                response.message = "{},{},{}".format(int(rgb[0]), int(rgb[1]), int(rgb[2]))
        return response

Set up autonomous line-following game.

set_threshold_srv_callback:

    def set_running_srv_callback(self, request, response):
        self.get_logger().info('\033[1;32m%s\033[0m' % "set_running")
        with self.lock:
            self.is_running = request.data
            self.empty = 0
            if not self.is_running:
                self.mecanum_pub.publish(Twist())
        response.success = True
        response.message = "set_running"
        return response

Set color threshold service.

lidar_callback:

    def lidar_callback(self, lidar_data):
        # 数据大小 = 扫描角度/每扫描一次增加的角度(data size= scanning angle/ the increased angle per scan)
        if self.lidar_type != 'G4':
            min_index = int(math.radians(MAX_SCAN_ANGLE / 2.0) / lidar_data.angle_increment)
            max_index = int(math.radians(MAX_SCAN_ANGLE / 2.0) / lidar_data.angle_increment)
            left_ranges = lidar_data.ranges[:max_index]  # 左半边数据(left data)
            right_ranges = lidar_data.ranges[::-1][:max_index]  # 右半边数据(right data)
        elif self.lidar_type == 'G4':
            min_index = int(math.radians((360 - MAX_SCAN_ANGLE) / 2.0) / lidar_data.angle_increment)
            max_index = int(math.radians(180) / lidar_data.angle_increment)
            left_ranges = lidar_data.ranges[min_index:max_index][::-1]  # 左半边数据 (the left data)
            right_ranges = lidar_data.ranges[::-1][min_index:max_index][::-1]  # 右半边数据 (the right data)

        # 根据设定取数据(Get data according to settings)
        angle = self.scan_angle / 2
        angle_index = int(angle / lidar_data.angle_increment + 0.50)
        left_range, right_range = np.array(left_ranges[:angle_index]), np.array(right_ranges[:angle_index])

        left_nonzero = left_range.nonzero()
        right_nonzero = right_range.nonzero()
        left_nonan = np.isfinite(left_range[left_nonzero])
        right_nonan = np.isfinite(right_range[right_nonzero])
        # 取左右最近的距离(Take the nearest distance left and right)
        min_dist_left_ = left_range[left_nonzero][left_nonan]
        min_dist_right_ = right_range[right_nonzero][right_nonan]
        if len(min_dist_left_) > 0 and len(min_dist_right_) > 0:
            min_dist_left = min_dist_left_.min()
            min_dist_right = min_dist_right_.min()
            if min_dist_left < self.stop_threshold or min_dist_right < self.stop_threshold:
                self.stop = True
            else:
                self.count += 1
                if self.count > 5:
                    self.count = 0
                    self.stop = False

Lidar callback function, reorganizes and collates the data read, and determines whether to stop moving forward.

image_callback:

    def image_callback(self, ros_image):
        cv_image = self.bridge.imgmsg_to_cv2(ros_image, "rgb8")
        rgb_image = np.array(cv_image, dtype=np.uint8)
        self.image_height, self.image_width = rgb_image.shape[:2]
        result_image = np.copy(rgb_image)  # 显示结果用的画面 (the image used to display the result)
        with self.lock:
            # 颜色拾取器和识别巡线互斥, 如果拾取器存在就开始拾取(color picker and line recognition are exclusive. If there is color picker, start picking)
            if self.color_picker is not None:  # 拾取器存在(color picker exists)
                try:
                    target_color, result_image = self.color_picker(rgb_image, result_image)
                    if target_color is not None:
                        self.color_picker = None
                        self.follower = LineFollower(target_color, self)
                        self.get_logger().info("target color: {}".format(target_color))
                except Exception as e:
                    self.get_logger().error(str(e))
            else:
                twist = Twist()
                twist.linear.x = 0.15
                if self.follower is not None:
                    try:
                        result_image, deflection_angle = self.follower(rgb_image, result_image, self.threshold)
                        if deflection_angle is not None and self.is_running and not self.stop:
                            self.pid.update(deflection_angle)
                            twist.angular.z = common.set_range(-self.pid.output, -1.0, 1.0)
                            self.mecanum_pub.publish(twist)
                        elif self.stop:
                            self.mecanum_pub.publish(Twist())
                        else:
                            self.pid.clear()
                    except Exception as e:
                        self.get_logger().error(str(e))
        if self.debug:
            if self.image_queue.full():
                # 如果队列已满，丢弃最旧的图像
                self.image_queue.get()

Camera callback function, invokes the color picker based on the read data, and moves the robot according to the recognized line using PID control.