# 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](3.Remote_Tool_Installation_Connection.md#docker-container-introduction-and-entry) and [3.4 ROS Version Switch Tool Guide](3.Remote_Tool_Installation_Connection.md#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.
```bash
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](../_static/source_code/ros2/advanced_functions.zip)**
(1) Import Function Package
{lineno-start=3}
```py
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.
{lineno-start=176}
```py
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.
{lineno-start=189}
```py
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.
{lineno-start=195}
```py
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.
{lineno-start=198}
```py
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.
{lineno-start=202}
```py
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.
{lineno-start=213}
```py
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.
{lineno-start=213}
```py
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:
```bash
cd ~/ros2_ws/src/example/example/advanced_functions
```
(2) Use the vim editor to open the program file:
```bash
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:
```bash
:wq
```
(6) Run the command below to restart the program and observe the updated behavior:
```bash
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:
```bash
~/.stop_ros.sh
```
(2) Start the USB camera node with the following command:
```bash
ros2 launch peripherals usb_cam.launch.py
```
(3) Open a new terminal and navigate to the LAB Tool directory:
```bash
cd /home/ubuntu/software/lab_tool
```
(4) Run the PC software:
```bash
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:
```bash
cd software/lab_tool/ && vim lab_config.yaml
```
(11) To test the updated settings, restart the program with the command below:
```bash
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](3.Remote_Tool_Installation_Connection.md#docker-container-introduction-and-entry) and [3.4 ROS Version Switch Tool Guide](3.Remote_Tool_Installation_Connection.md#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):
```bash
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](../_static/source_code/ros2/advanced_functions.zip)**
* **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.
{lineno-start=103}
```python
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.
```bash
cd ros2_ws/src/example/example/advanced_functions/
```
(2) Next, enter the following command to open the program file using the Vim editor.
```bash
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.
```bash
:wq
```
(6) Input the following command to restart the game and check PuppyPi's performance.
```bash
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](#anchor_24_1_5)
After completing the modification, run the program to see the updated effect.
```bash
ros2 launch example negotiate_stairs_demo.launch.py model:=1
```