24. ROS2-AI Stair Recognition and Negotiating Course

24.1 Stair Recognition

24.1.1 Program Logic

Before the game, we need to DIY a stair. We can use 20mm height items as stair, such as board and sturdy box. And paste the red electrical tape on the border, because PuppyPi recognize the stair through recognizing the color.

Stair recognition process is as follow.

Firstly, program to recognize the color of line. Use Lab color space to convert the image from RGB into Lab. Then, perform binaryzation, corrosion, dilation, etc., on the image to obtain the contour which contains the target color. Next, mark the contour with rectangle.

Then, acquire the diagonal point of the rectangle and draw the center of the line.

Lastly, display the information about the line center on the terminal.

24.1.2 Operation Steps

Note

The input command should be case and space sensitive.

(1) Power on the robot, then follow the steps in 3.3 Docker Container Introduction and Entry and 3.4 ROS Version Switch Tool Guide to connect via the VNC remote control software and switch to the ROS 2 environment.

(2) Click to open Terminator ROS2 terminal.

(3) Input the command below and press Enter to start the game.

ros2 launch example negotiate_stairs_demo.launch.py model:=0

(4) If want to close this game, we can press “Ctrl+C”. If it fails to close the game, please try again.

24.1.3 Program Outcome

The program is default to detect red.

Paste the red electrical tape on the boarder of the stair, and then place the stair in front of PuppyPi. After the game starts, PuppyPi will recognize the line. When recognized, it will be framed on the camera returned image and its center will be drawn.

24.1.4 Program Analysis

The source code of this program is stored in ros2_ws/src/example/example/advanced_functions/negotiate_stairs_demo.py

(1) Import Function Package

 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
import sys
import cv2
import time
import math
import numpy as np
from enum import Enum
import yaml
from sdk import Misc
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile
from sensor_msgs.msg import Image
from std_srvs.srv import Empty, SetBool
from interfaces.srv import *
from puppy_control_msgs.msg import Velocity, Pose, Gait
from puppy_control_msgs.srv import SetRunActionName
from cv_bridge import CvBridge

Using import statements to import the required modules: math provides a range of mathematical functions and constants for related calculations; rospy is used for ROS communication. from object_tracking.srv import * imports services related to object tracking. from puppy_control.msg import Velocity, Pose, Gait imports services for controlling and transmitting the velocity, pose, and gait of the robot.

  • Image Processing

(1) Gaussian Filtering

Before converting the image from RGB into Lab space, denoise the image and use GaussianBlur() function in cv2 library for Gaussian filtering.

176
frame_gb = cv2.GaussianBlur(frame_resize, (3, 3), 3)

The meaning of the parameters in bracket is as follow:

① The first parameter frame_resize is the input image

② The second parameter (3, 3) is the size of Gaussian kernel

③ The third parameter 3 is the allowable range of variation around the average in Gaussian filtering. The larger the value, the larger the allowable range of variation

(2) Binaryzation Processing

Adopt inRange() function in cv2 library to perform binaryzation on the image.

189
190
191
192
193
	            frame_mask = cv2.inRange(
                    frame_lab,
                    tuple(self.color_range_list[detect_color]['min']),
                    tuple(self.color_range_list[detect_color]['max'])
                )

The first parameter in the bracket is the input image. The second and the third parameters respectively are the lower limit and upper limit of the threshold. When the RGB value of the pixel is between the upper limit and lower limit, the pixel is assigned 1, otherwise, 0.

(3) Open Operation and Close Operation

To reduce interference and make the image smoother, it is necessary to process the image.

195
196
opened = cv2.morphologyEx(frame_mask, cv2.MORPH_OPEN, np.ones((6, 6), np.uint8))
closed = cv2.morphologyEx(opened, cv2.MORPH_CLOSE, np.ones((6, 6), np.uint8))

cv2.MORPH_OPEN refers to open operation where corrosion will be conducted first, then dilation. cv2.MORPH_CLOSE indicates close operation where dilation will be conducted first, then corrosion.

Take opened = cv2.morphologyEx(frame_mask, cv2.MORPH_OPEN, np.ones((6, 6), np.uint8)) for example. The meaning of the parameters in bracket is as follow.

① The first parameter frame_mask is the input image.

② The second parameter cv2.MORPH_OPEN refers to processing method, open operation.

③ The third parameter np.ones((6, 6), np.uint8) is frame size.

(4) Acquire the Maximum Contour

After processing the image, acquire the contour of the target to be recognized, which involves findContours() function in cv2 library.

198
199
200
	    if detect_color:
            cnts = cv2.findContours(closed, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_TC89_L1)[-2]
            cnt_large, area_max = getAreaMaxContour(cnts)

The first parameter in parentheses is the input image; the second parameter is the retrieval mode of the contour; the third parameter is the approximation method of the contour.

Find the contour of the maximum area among the obtained contours. To avoid interference, please set a minimum value. Only when the area is greater than this value, the target contour is valid.

202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
	        if cnt_large is not None:
                rect = cv2.minAreaRect(cnt_large)
                box = np.int0(cv2.boxPoints(rect))
                centerX = rect[0][0]
                centerY = rect[0][1]
                centerX = int(Misc.map(centerX, 0, self.image_size[0], 0, img_w))
                centerY = int(Misc.map(centerY, 0, self.image_size[1], 0, img_h))
                for i in range(4):
                    box[i, 1] = int(Misc.map(box[i, 1], 0, self.image_size[1], 0, img_h))
                    box[i, 0] = int(Misc.map(box[i, 0], 0, self.image_size[0], 0, img_w))

                cv2.drawContours(img, [box], -1, (0, 0, 255, 255), 2)
                self.target_centre_point = [centerX, centerY]
                cv2.circle(img, (centerX, centerY), 5, (0, 0, 255), -1)
                # 打印目标颜色和中心点
                self.get_logger().info(f"检测到目标颜色: {detect_color}, 中心点: {self.target_centre_point}")

  • Acquire the Position

(1) Frame the Line

Call drawContours() function to set the rectangle pattern and frame the line.

213
 cv2.drawContours(img, [box], -1, (0, 0, 255, 255), 2)

(2) Draw the Center

Next, acquire the diagonal points of the rectangle, and draw the line center through circle() function.

213
214
215
216
217
218
219
220
221
222
	            cv2.drawContours(img, [box], -1, (0, 0, 255, 255), 2)
                self.target_centre_point = [centerX, centerY]
                cv2.circle(img, (centerX, centerY), 5, (0, 0, 255), -1)
                # 打印目标颜色和中心点
                self.get_logger().info(f"检测到目标颜色: {detect_color}, 中心点: {self.target_centre_point}")
            else:
                self.target_centre_point = None
                self.get_logger().info(f"未检测到目标颜色: {detect_color}")

        return img

24.1.5 Function Extension

  • Modify Default Recognition Color

By default, the program is configured to recognize the color red as the target. The following steps demonstrate how to modify the default target color to green:

(1) Open a terminal and run the following command to enter the directory containing the program:

cd ~/ros2_ws/src/example/example/advanced_functions

(2) Use the vim editor to open the program file:

vim negotiate_stairs_demo.py

(3) Find the line of code responsible for setting the target color. Refer to the image below for guidance.

Note

In vim, you can jump to a specific line by typing the line number, then pressing Shift + G.(Note: The line number shown in the image is for reference only. Please locate the actual line in your file.)

(4) Press the i key to enter insert mode, and change the line to: self.set_target(‘green’)

(5) After editing, press the Esc key, then type the following to save and exit:

:wq

(6) Run the command below to restart the program and observe the updated behavior:

ros2 launch example negotiate_stairs_demo.launch.py model:=0

  • Adding a New Recognizable Color

In addition to the default colors configured in the program, users can add custom colors for recognition. This section provides step-by-step instructions for adding yellow as a new detectable color:

(1) Open a terminal and run the following command to enter the directory containing the program:

~/.stop_ros.sh

(2) Start the USB camera node with the following command:

ros2 launch peripherals usb_cam.launch.py

(3) Open a new terminal and navigate to the LAB Tool directory:

cd /home/ubuntu/software/lab_tool

(4) Run the PC software:

python3 main.py

(5) Click the “Add” button located at the bottom right corner of the interface.

(6) In the popup dialog, enter “yellow” as the new color name and click “OK”.

(7) From the color selection panel at the bottom right, select “yellow”.

(8) Place a yellow object within the camera’s field of view. Adjust the L, A, and B sliders until the yellow area appears white on the left side of the interface, while all other areas appear black.

(9) Once you’re satisfied with the result, click the “Save” button to store the LAB threshold values. Then, close the LAB Tool.

(10) To ensure the new color settings were saved correctly, open the configuration file using the following commands:

cd software/lab_tool/ && vim lab_config.yaml

(11) To test the updated settings, restart the program with the command below:

ros2 launch example negotiate_stairs_demo.launch.py model:=0

24.2 Stair Negotiating

24.2.1 Program Logic

Before the game, we need to DIY a stair. We can use 20mm height items as stair, such as board and sturdy box. And paste the red electrical tape on the border. Then PuppyPi will recognize the red tape and approach it to finish negotiating the stair.

Stair negotiating process is as follow.

Firstly, program to recognize the color of line. Use Lab color space to convert the image from RGB into Lab. Then, perform binaryzation, corrosion, dilation, etc., on the image to obtain the contour which contains the target color. Next, mark the contour with rectangle. And draw the center of line with red dot.

Next, control PuppyPi to approach the line center to come closer to the stair.

Lastly, control PuppyPi to negotiate the stair.

24.2.2 Operation Steps

Note

The input command should be case and space sensitive.

(1) Power on the robot, then follow the steps in 3.3 Docker Container Introduction and Entry and 3.4 ROS Version Switch Tool Guide to connect via the VNC remote control software and switch to the ROS 2 environment.

(2) Click to open Terminator ROS2 terminal. Enter the commands to activate the feature (two lines of commands, entered one line at a time):

ros2 launch example negotiate_stairs_demo.launch.py model:=1

(3) To exit this feature, press “Ctrl+C” in the LX terminal interface. If the shutdown fails, try pressing “Ctrl+C” multiple times.

24.2.3 Program Outcome

Note

The program is default to detect red.

Paste the red electrical tape on the boarder of the stair, and then place the stair in front of PuppyPi. After the game starts, PuppyPi will recognize the line, and then approach the center of the line to negotiate the stair.

24.2.4 Program Analysis

The source code for this program is located within the Docker container.

ros2_ws/src/example/example/advanced_functions/negotiate_stairs_demo.py

  • Approach the Stair

In the first lesson, we introduced how to identify stairs. Next, you can control the robot to approach the stairs based on the center coordinate information of the lines, as shown in the diagram below:

In the code, an enumerated class named PuppyStatus is defined, which includes five states. These states represent different actions or events, such as:

LOOKING_FOR: Searching for the stairs

FOUND_TARGET: Target stairs found

DOWN_STAIRS: Descending the stairs

STOP: Stopping

END: Ending

The getAreaMaxContour function is used to find the largest contour by area from a set of contours. The function iterates through all contours, calculates the area of each, and records the largest contour and its area. If a contour’s area is greater than or equal to 5, it is considered valid and returned as the result.

  • Negotiate the Stair

After PuppyPi approaches the stair, call the built-in action group “up_stairs_2cm.d6ac” to control it to negotiate the stair.

103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
	def move(self):
        if not self.is_running:
            return

        with self.lock:
            if self.model == 0:
	                # model=0：仅识别，不控制运动
                return

            # 状态机逻辑
            if self.puppyStatus == PuppyStatus.LOOKING_FOR:
                if self.target_centre_point and self.target_centre_point[1] > 400:
                    self.puppyStatus = PuppyStatus.FOUND_TARGET
                    velocity_msg = Velocity(x=0.0, y=0.0, yaw_rate=0.0)
                    self.velocity_publisher.publish(velocity_msg)
                    self.up_stairs_time = time.time()
                    self.action_group_executed = False
                    self.get_logger().info('发现目标，准备执行上台阶动作')
                else:
                    velocity_msg = Velocity(x=10.0, y=0.0, yaw_rate=0.0)
                    self.velocity_publisher.publish(velocity_msg)
                    self.get_logger().info('未发现目标，继续向前移动')

            elif self.puppyStatus == PuppyStatus.FOUND_TARGET:
                if not self.action_group_executed:
                    self.run_action_group('up_stairs_2cm.d6ac', True)
                    self.action_group_executed = True
                    self.get_logger().info('执行上台阶动作组')

                if time.time() - self.up_stairs_time > 25:
                    self.puppyStatus = PuppyStatus.DOWN_STAIRS
                    pose_msg = Pose(
                        stance_x=0.0, stance_y=0.0, x_shift=0.0,
                        height=0.3, roll=0.0, pitch=0.0, yaw=0.0, run_time=500
                    )
                    self.pose_publisher.publish(pose_msg)
                    self.get_logger().info('超时，切换到下台阶状态')

            elif self.puppyStatus == PuppyStatus.DOWN_STAIRS:
                velocity_msg = Velocity(x=14.0, y=0.0, yaw_rate=0.0)
                self.velocity_publisher.publish(velocity_msg)
                self.get_logger().info('正在下台阶')
                self.puppyStatus = PuppyStatus.END

            elif self.puppyStatus == PuppyStatus.END:
                velocity_msg = Velocity(x=0.0, y=0.0, yaw_rate=0.0)
                self.velocity_publisher.publish(velocity_msg)
                self.get_logger().info('任务完成，停止移动')

            # 状态变化日志
            if self.puppyStatusLast != self.puppyStatus:
                self.get_logger().info(f'当前状态: {self.puppyStatus}')
            self.puppyStatusLast = self.puppyStatus

Publishing Speed Messages

In the move method, speed messages are published using the command self.velocity_publisher.publish(velocity_msg). This is similar to the example where self.rect = self.rect.move(self.speed) is used to move an object by updating its speed. Both approaches rely on publishing speed messages to control movement.

State Transitions: The code determines the next action by checking the value of self.puppyStatus. This is comparable to how the move function in the example uses the current state to adjust the ball’s movement.

Logging: The code utilizes self.get_logger().info() to log information, which is consistent with the logging approach mentioned in the example.

Behavior Based on self.puppyStatus:

① When self.puppyStatus is PuppyStatus.LOOKING_FOR:

  • If self.target_centre_point exists and its y coordinate is greater than 400, the robot’s status is updated to PuppyStatus.FOUND_TARGET. A speed message is then published to stop the robot.

  • If self.target_centre_point does not exist or its y coordinate is not greater than 400, a speed message is published to move the robot with an x velocity of 10.0.

② When self.puppyStatus is PuppyStatus.FOUND_TARGET:

  • If self.action_group_executed is False, the robot executes an action group named 'up_stairs_2cm.d6ac' and sets self.action_group_executed to True.

③ When self.puppyStatus is PuppyStatus.DOWN_STAIRS:

  • A speed message is published to move the robot dog forward with an x velocity of 14.0.

  • The robot’s status is then updated to PuppyStatus.END, and a log message is recorded.

24.2.5 Function Extension

  • Close Debugging Interface

As the continuous refresh of debugging interface will occupy CPU of Raspberry Pi, we can close debugging interface to tackle choppy running.

(1) Input the command below and press Enter to edit the stair negotiating program.

cd ros2_ws/src/example/example/advanced_functions/

(2) Next, enter the following command to open the program file using the Vim editor.

vim negotiate_stairs_demo.py

(3) Next, jump to this line of code.

Note

we can input the line code and press “Shift+G” to jump to the corresponding line. The numbering of code positions in the diagram is for reference only, please refer to the actual positions.

(4) Press “i” key to enter editing mode. Then add “#” in front of the codes in the red frame to comment.

(5) After modification, press “Esc” and input “:wq” and press Enter to save and exit editing.

:wq

(6) Input the following command to restart the game and check PuppyPi’s performance.

ros2 launch example negotiate_stairs_demo.launch.py model:=1

(7) If you need to view the debugging screen again (real-time feedback from the camera), you can uncomment the content boxed in step 3), i.e., remove the “#” in front of the code, then save, as shown in the following figure:

  • Change Target Recognized Color

The default color of the step edge lines in the program is red. To change the color—for example, to black—please refer to section 24.1.5 Function Extension After completing the modification, run the program to see the updated effect.

ros2 launch example negotiate_stairs_demo.launch.py model:=1