智能小车-轮趣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()

  • 2
    点赞
  • 2
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

洛杉矶县牛肉板面

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值