上一届大佬的 红绿灯识别代码,此代码需要在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之间的值变成255kernel = 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=0lowerbS=0lowerbV=0upperbH=255upperbS=255upperbV=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.shapesearch_top = h - 150# 原search_top = h-20search_bot = h#??????mask[0:search_top, 0:w] = 0mask[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.zd_erro=erro-last_erro#设置机器人的线速度为0.18self.twist.linear.x = 0.18if erro!=0:#这里需要改self.twist.angular.z = -float(erro)*0.005-float(d_erro)*0.000else :self.twist.angular.z = 0last_erro=erroelse:self.twist.linear.x = 0self.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)#显示图像#显示掩码图像maskcv2.imshow("Adjust_hsv", mask)#通过cv2.waitKey(3)函数暂停3毫秒,以便用户可以看到图像。cv2.waitKey(3)rospy.init_node("opencv")
follower = Follower()
rospy.spin()