基于旭日派的Ros系统小车开发——车辆循迹功能开发

本人相关文章推荐：

——————————————————————————
基于旭日派的Ros系统小车开发——实时人脸识别脚本-优快云博客

基于旭日派的Ros系统小车的再开发——使用python脚本调用MIPI相机（学习笔记）-优快云博客

基于旭日派的Ros系统小车的再开发——使用python脚本Astra调用深度相机（学习笔记）-优快云博客

————————————————————————————————

1. 沿中心实线运动

1.1 循迹程序

######################
# The program will call the MiPi camera to capture image in real time, recognize soid yellow lines, and move along them.
#########################

import cv2
import numpy as np
import time
from SunriseRobotLib import Mipi_Camera
from SunriseRobotLib import SunriseRobot

# 创建SunriseRobot对象 bot 
bot = SunriseRobot()

# 初始化摄像头
width = 320
height = 240
g_camera = Mipi_Camera(width, height, debug=True)

if g_camera.isOpened():
    print("Open Camera OK!")
else:
    print("Fail To Open Camera!")

# 定义小车的速度和阈值
v_x = 0.3  # 小车纵向速度
v_y = 0.2  # 小车横向速度
v_z = 0.1  # 小车旋转速度
threshold = 20  # 阈值

# 定义变量来判断是否按下空格键
stopped = False

try:
    while g_camera.isOpened():
        ret, frame = g_camera.read()
        if not ret:
            print("Camera Read Fail!")
            break

        # 将图像转换为HSV颜色空间
        hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
        # 定义黄色阈值范围
        lower_yellow = np.array([20, 100, 100])
        upper_yellow = np.array([30, 255, 255])
        # 根据阈值创建掩模
        yellow_mask = cv2.inRange(hsv, lower_yellow, upper_yellow)

        # 形态学操作，去除小的噪声
        kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (5, 5))
        yellow_mask = cv2.morphologyEx(yellow_mask, cv2.MORPH_OPEN, kernel)
        # 填充小的空洞
        yellow_mask = cv2.morphologyEx(yellow_mask, cv2.MORPH_CLOSE, kernel)

        # 找到黄色区域的轮廓
        contours, hierarchy = cv2.findContours(yellow_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

        # 在图像上画出黄色区域的轮廓
        cv2.drawContours(frame, contours, -1, (0, 255, 0), 2)

        # 判断是否检测到黄色区域
        if len(contours) > 0:
            # 计算黄色区域的质心
            M = cv2.moments(contours[0])
            if M["m00"] > 0:
                cx = int(M["m10