智能小车-轮趣wheeltec(原版代码)

上一届大佬的 红绿灯识别代码，此代码需要在ubuntu系统下，与ROS配合使用：

Xtrak 塔克小车巡线代码以及红绿灯识别相关小改动_search_top=0 mask-CSDN博客

line.follow原版 源代码：
 

#------------------------------------------------------------------------------------------
#!/usr/bin/env python
# coding=utf-8

#######################################################################
# 巡线   源代码
#######################################################################

import rospy
from sensor_msgs.msg import Image #表示图像数据
import cv2, cv_bridge #在ROS图像消息和OpenCV图像之间的桥接库
import numpy
from geometry_msgs.msg import Twist #消息类型，表示线性速度和角速度，控制机器人移动

#颜色跟踪 或 机器人控制算法中的误差计算或状态更新
last_erro=0

#空函数，用作cv2.createTrackbar的回调函数。
#在OpenCV的滑动条（trackbar）上移动滑块时，函数会被调用。实际它不会被调用
def nothing(s):
    pass

#定义不同颜色在HSV色彩空间中的范围，每个颜色由六个值定义；可考虑多定义几个颜色 或者放宽颜色范围，以识别更多种类的颜色
col_black = (0,0,0,180,255,46)# black
col_red = (0,100,80,10,255,255)# red
col_blue = (90,90,90,110,255,255)# blue
col_green= (65,70,70,85,255,255)# green
col_yellow = (26,43,46,34,255,255)# yellow

# 创建OpenCV窗口和滑动条
#创建一个名为'Adjust_hsv'的OpenCV窗口，并且设置窗口的大小可以自由调整(cv2.WINDOW_NORMAL)
cv2.namedWindow('Adjust_hsv',cv2.WINDOW_NORMAL)

#定义字符串，表示不同颜色选项，每个选项对应一个索引
Switch = '0:Red\n1:Green\n2:Blue\n3:Yellow\n4:Black'

#在'Adjust_hsv'窗口中创建一个滑动条。滑动条名称:Switch，窗口名称:'Adjust_hsv'
# 滑动条的初始值是0，最大值是4 ,回调函数是nothing。
cv2.createTrackbar(Switch,'Adjust_hsv',0,4,nothing)


#处理从摄像头获取的图像，根据图像中的颜色信息来发布机器人的移动指令
class Follower:
    def __init__(self):

        #初始化一个CvBridge对象，用于在ROS图像消息和OpenCV图像之间进行转换
        self.bridge = cv_bridge.CvBridge()

        #cv2.namedWindow("window", 1)
        # 订阅usb摄像头图像 该话题发布的是摄像头捕获的图像数据
        self.image_sub = rospy.Subscriber("/usb_cam/image_raw", Image, self.image_callback)

        # self.image_sub = rospy.Subscriber("cv_bridge_image", Image, self.image_callback)
        # 订阅深度相机
        # self.image_sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_callback)
        # self.image_sub = rospy.Subscriber("/camera/depth/image_raw", Image,self.image_callback)

        #发布机器人的移动指令。发布的话题cmd_vel_ori，消息类型：Twist，队列大小为1。
        self.cmd_vel_pub = rospy.Publisher("cmd_vel_ori", Twist, queue_size=1)
        #初始化Twist消息对象，用于存储机器人的线性和角速度指令
        self.twist = Twist()

    def image_callback(self, msg):
        global last_erro #全局变量

        #转换图像格式 使用CvBridge对象将ROS图像消息转换为OpenCV图像格式。
        image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')

        #调整图像大小 为提高帧率(图像处理速度) 插值
        image = cv2.resize(image, (320,240), interpolation=cv2.INTER_AREA)

        # hsv将RGB图像分解成色调H，饱和度S，明度V,易基于颜色范围进行图像分割。
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)


        # 颜色的范围  # 第二个参数：lower指的是图像中低于这个lower的值，图像值变为0
        # 第三个参数：upper指的是图像中高于这个upper的值，图像值变为0
        # 而在lower～upper之间的值变成255

        kernel = numpy.ones((5,5),numpy.uint8)

        #对HSV图像进行腐蚀和膨胀操作，以消除噪声和强调颜色边界
        hsv_erode = cv2.erode(hsv,kernel,iterations=1)
        hsv_dilate = cv2.dilate(hsv_erode,kernel,iterations=1)

        #读取滑动条位置，从名为Adjust_hsv的窗口中获取名为Switch的滑动条的位置。
        m=cv2.getTrackbarPos(Switch,'Adjust_hsv')

        #根据滑动条的位置m，从预定义的颜色范围中选择相应的HSV值范围
        if m == 0:
            lowerbH=col_red[0]
            lowerbS=col_red[1]
            lowerbV=col_red[2]
            upperbH=col_red[3]
            upperbS=col_red[4]
            upperbV=col_red[5]
        elif m == 1:
            lowerbH=col_green[0]
            lowerbS=col_green[1]
            lowerbV=col_green[2]
            upperbH=col_green[3]
            upperbS=col_green[4]
            upperbV=col_green[5]
        elif m == 2:
            lowerbH=col_blue[0]
            lowerbS=col_blue[1]
            lowerbV=col_blue[2]
            upperbH=col_blue[3]
            upperbS=col_blue[4]
            upperbV=col_blue[5]
        elif m == 3:
            lowerbH=col_yellow[0]
            lowerbS=col_yellow[1]
            lowerbV=col_yellow[2]
            upperbH=col_yellow[3]
            upperbS=col_yellow[4]
            upperbV=col_yellow[5]
        elif m == 4:
            lowerbH=col_black[0]
            lowerbS=col_black[1]
            lowerbV=col_black[2]
            upperbH=col_black[3]
            upperbS=col_black[4]
            upperbV=col_black[5]
        else:
            lowerbH=0
            lowerbS=0
            lowerbV=0
            upperbH=255
            upperbS=255
            upperbV=255


        #处理从摄像头获取的图像，并通过颜色范围来检测特目标物体

        # 计算该对象在图像中的位置，并根据位置信息控制机器人的移动

        #创建掩码图像：目标颜色范围内的像素值为255（白色），其他像素值为0（黑色）
        mask=cv2.inRange(hsv_dilate,(lowerbH,lowerbS,lowerbV),(upperbH,upperbS,upperbV))

        #应用掩码到原始图像
        masked = cv2.bitwise_and(image, image, mask=mask)


        #绘制指示并计算重心
        #在图像某处绘制一个指示，因为只考虑20行宽的图像，所以使用numpy切片将以外的空间区域清空
        h, w, d = image.shape

        search_top = h - 150
        # 原search_top = h-20

        search_bot = h

        #??????
        mask[0:search_top, 0:w] = 0
        mask[search_bot:h, 0:w] = 0

        # 计使用cv2.moments函数计算mask图像的重心(cx,cy)，即几何中心
        M = cv2.moments(mask)

        #在图像上绘制一个以重心为圆心的红色圆圈，用于可视化重心的位置
        if M['m00'] > 0:
            cx = int(M['m10']/M['m00'])
            cy = int(M['m01']/M['m00'])
            cv2.circle(image, (cx, cy), 10, (255, 0, 255), -1)
            #cv2.circle(image, (cx-75, cy), 10, (0, 0, 255), -1)
            #cv2.circle(image, (w/2, h), 10, (0, 255, 255), -1)


            #计算偏移量和发布移动指令

            #????????
            if cv2.circle:
            # 计算图像中心线和目标指示线中心的距离

                #15可以调整
                erro = cx - w/2-15 #计算重心cx和图像中心线w/2的偏移量erro

                #根据偏移量erro和偏移量的变化d_erro来设置机器人的角速度self.twist.angular.z
                d_erro=erro-last_erro

                #设置机器人的线速度为0.18
                self.twist.linear.x = 0.18


                if erro!=0:

                    #这里需要改
                    self.twist.angular.z = -float(erro)*0.005-float(d_erro)*0.000
                else :
                    self.twist.angular.z = 0
                last_erro=erro
        else:
            self.twist.linear.x = 0
            self.twist.angular.z = 0

        # 发布移动指令
        self.cmd_vel_pub.publish(self.twist)


        #cv2.imshow("window", image)
        #cv2.imshow("window1", hsv)
        #cv2.imshow("window2", hsv_erode)
        #cv2.imshow("window3", hsv_dilate)
        #cv2.imshow("window4", mask)


        #显示图像
        #显示掩码图像mask
        cv2.imshow("Adjust_hsv", mask)
        #通过cv2.waitKey(3)函数暂停3毫秒，以便用户可以看到图像。
        cv2.waitKey(3)


rospy.init_node("opencv")
follower = Follower()
rospy.spin()