Xtrak 塔克小车巡线代码以及红绿灯识别相关小改动
做的小改动：
只更改了xtark_follow_line.py
增加了一次HSV分割来做红绿灯识别（建议到时候用手机或者平板显示红色或者绿色图片），红绿图片：
将检测道路位置的方式从重心变为最接近屏幕中心的道路像素点。
检测不到道路之后会检测上面一段区域，来处理断线
修改速度处理函数来使得运行更平稳
答辩的PPT下载：比较简洁，非常建议使用自带的PowerPoint打开，（因为用了平滑切换）
里面有一点演示视频（小车部分）
改动前原始代码：
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import rospy
import cv2
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
import numpy
from geometry_msgs.msg import Twist
# 色彩矩阵，当前测试出的几种色彩
color_arry = [[100,55,90,130,215,170], # red
[0,220,160,20,255,220],# blue
[20,155,45,60,195,85], # green
[75,195,195,115,235,255], # yellow
[0,0,0,150,40,46]] # black
# 记录颜色跟随时的误差值
diff= 0
d_diff= 0
last_diff= 0
def line_follow_init():
#define topic publisher and subscriber
global color_mode, bridge, image_sub, mask_pub, result_pub, pub_cmd
bridge = CvBridge()
image_sub = rospy.Subscriber("/camera/rgb/image_raw", Image, callback)
mask_pub = rospy.Publisher("/mask_image", Image, queue_size=1)
result_pub = rospy.Publisher("/line_image", Image, queue_size=1)
pub_cmd = rospy.Publisher('cmd_vel', Twist, queue_size=5)
color_mode = int(rospy.get_param('~color_mode'))
rospy.loginfo('color_mode:%d', color_mode)
def callback(data):
global raw_image, mask_image
# convert ROS topic to CV image formart
# 将将ROS主题转换为CV图像格式
raw_image = bridge.imgmsg_to_cv2(data, "bgr8")
raw_image = cv2.resize(raw_image, (320,240), interpolation=cv2.INTER_AREA)#提高帧率
# 将图像从 RGB 转为 HSV
hsv_image = cv2.cvtColor(raw_image,cv2.COLOR_RGB2HSV)
# close operation to fit some little hole
# 创建一个5行5列的数组
kernel = numpy.ones((5,5),numpy.uint8)
# 对图片进行膨胀腐蚀操作
hsvimage_erode = cv2.erode(hsv_image,kernel,iterations=1)
hsvimag_dilate = cv2.dilate(hsvimage_erode,kernel,iterations=1)
# 设置图像的HSV阈值
# h_low s_low v_low
color_lower = numpy.array([color_arry[color_mode][0],color_arry[color_mode][1],color_arry[color_mode][2]])
# h_upper s_upper v_upper
color_upper = numpy.array([color_arry[color_mode][3],color_arry[color_mode][4],color_arry[color_mode][5]])
# 得到处理后的二值化图像
mask_image = cv2.inRange(hsvimag_dilate,color_lower,color_upper)
masked = cv2.bitwise_and(raw_image, raw_image, mask=mask_image)
# 图像处理函数
image_processing()
# 对原始图像进行处理，获得我们需要的图像部分
def image_processing():
res = raw_image
h, w, d = res.shape
search_top = h-20
search_bot = h
mask_image[0:search_top, 0:w] = 0
mask_image[search_bot:h, 0:w] = 0
# 计算二值化图像的重心，即几何中心
P = cv2.moments(mask_image)
# 得到可识别的颜色数据，进行识别和色差计算
if P['m00'] > 0:
ix = int(P['m10']/P['m00'])
iy = int(P['m01']/P['m00'])
cv2.circle(res, (ix, iy), 10, (255, 0, 255), -1)
if cv2.circle:
diff= ix - w/2-15
d_diff=diff-last_diff
twist_calculate(diff,d_diff)
else:
twist = Twist()
twist.linear.x = 0
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = 0
pub_cmd.publish(twist)
# 将实际图像和二值化图像通过话题发出
img_msg = bridge.cv2_to_imgmsg(res, encoding="bgr8")
img_msg.header.stamp = rospy.Time.now()
result_pub.publish(img_msg)
img_msg = bridge.cv2_to_imgmsg(mask_image, encoding="passthrough")
img_msg.header.stamp = rospy.Time.now()
mask_pub.publish(img_msg)
# 速度处理函数
def twist_calculate(twist_erro,twist_d_erro):
twist = Twist()
twist.linear.x = 0.2
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = 0
if twist_erro!=0:
twist.angular.z = -float(twist_erro)*0.005-float(twist_d_erro)*0.000
else:
twist.angular.z = 0
last_diff=diff
pub_cmd.publish(twist)
if __name__ == '__main__':
try:
# init ROS node
rospy.init_node("line_follow")
rospy.loginfo("Starting Line Follow node")
line_follow_init()
rospy.spin()
except KeyboardInterrupt:
print ("Shutting down cv_bridge_test node.")
cv2.destroyAllWindows()改动后代码：
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import time
import rospy
import cv2
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
import numpy
from geometry_msgs.msg import Twist
# 色彩矩阵，当前测试出的几种色彩
color_arry = [[100, 55, 90, 130, 215, 170], # red
[0, 220, 160, 20, 255, 220], # blue
[20, 155, 45, 60, 195, 85], # green
[75, 195, 195, 115, 235, 255], # yellow
[0, 0, 0, 150, 40, 46], ] # black
# 记录颜色跟随时的误差值
diff = 0
d_diff = 0
last_diff = 0
up_diff = 0
red_light = False # 遇见红灯
def line_follow_init():
# define topic publisher and subscriber
global color_mode, bridge, image_sub, mask_pub, result_pub, pub_cmd
bridge = CvBridge()
image_sub = rospy.Subscriber("/camera/rgb/image_raw", Image, callback)
mask_pub = rospy.Publisher("/mask_image", Image, queue_size=1)
result_pub = rospy.Publisher("/line_image", Image, queue_size=1)
pub_cmd = rospy.Publisher('cmd_vel', Twist, queue_size=5)
color_mode = int(rospy.get_param('~color_mode'))
rospy.loginfo('color_mode:%d', color_mode)
def callback(data):
global raw_image, mask_image, red_light
# convert ROS topic to CV image formart
# 将将ROS主题转换为CV图像格式
raw_image = bridge.imgmsg_to_cv2(data, "bgr8")
raw_image = cv2.resize(raw_image, (320, 240), interpolation=cv2.INTER_AREA) # 提高帧率
# 将图像从 RGB 转为 HSV
hsv_image = cv2.cvtColor(raw_image, cv2.COLOR_RGB2HSV)
# close operation to fit some little hole
# 创建一个5行5列的数组
kernel = numpy.ones((5, 5), numpy.uint8)
# 对图片进行膨胀腐蚀操作
# hsvimage_erode = cv2.erode(hsv_image, kernel, iterations=1)
# hsvimag_dilate = cv2.dilate(hsvimage_erode, kernel, iterations=1)
# 设置图像的HSV阈值
# h_low s_low v_low
color_lower = numpy.array([color_arry[color_mode][0], color_arry[color_mode][1], color_arry[color_mode][2]])
# h_upper s_upper v_upper
color_upper = numpy.array([color_arry[color_mode][3], color_arry[color_mode][4], color_arry[color_mode][5]])
# 得到处理后的二值化图像
mask_image = cv2.inRange(hsv_image, color_lower, color_upper)
# 红灯和绿灯的HSV并二值化
color_red_lower = numpy.array([100, 170, 120])
color_red_upper = numpy.array([130, 240, 240])
color_green_lower = numpy.array([65, 220, 150])
color_green_upper = numpy.array([80, 230, 220])
# 自行设置，让红绿灯出现在非0条内
h, w = mask_image.shape
search_top = h - 140
search_bot = h - 80
# 没遇到红灯找红灯
if not red_light:
mask_red_image = cv2.inRange(hsv_image, color_red_lower, color_red_upper)
# 自行调参
mask_red_image = cv2.erode(mask_red_image, kernel, iterations=1)
if numpy.amax(mask_red_image) > 0:
print("遇到红灯")
red_light = True
pass
else: # 遇到找绿灯
mask_green_image = cv2.inRange(hsv_image, color_green_lower, color_green_upper)
# 自行调参
mask_green_image = cv2.erode(mask_green_image, kernel, iterations=1)
if numpy.amax(mask_green_image) > 0:
print("遇到绿灯")
red_light = False
pass
if red_light: # 红灯停
twist = Twist()
twist.linear.x = 0
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
pub_cmd.publish(twist)
else:
# masked = cv2.bitwise_and(raw_image, raw_image, mask=mask_image)
# 图像处理函数
image_processing()
# 对原始图像进行处理，获得我们需要的图像部分
def image_processing():
global mask_image, up_diff, last_diff, diff
res = raw_image
h, w, d = res.shape
search_top = h - 20
search_bot = h
img_up = mask_image.copy()
mask_image[0:search_top, 0:w] = 0
mask_image[search_bot:h, 0:w] = 0
# 计算二值化图像
P = [None, None]
for i in range(w // 2 - 17):
if P[0] is not None:
break
for j in range(20):
if P[0] is not None:
break
if not mask_image[search_top + j, w // 2 - 15 + i] == 0:
P = [search_top + j, w // 2 - 15 + i]
elif not mask_image[search_top + j, w // 2 - 15 - i] == 0:
P = [search_top + j, w // 2 - 15 - i]
# 得到可识别的颜色数据，进行识别和色差计算
if P[0] is not None: # 如果在最下面找到线
ix = int(P[1])
iy = int(P[0])
cv2.circle(res, (ix, iy), 10, (255, 0, 255), -1)
if cv2.circle:
diff = ix - w / 2
d_diff = diff - last_diff
twist_calculate(diff, d_diff)
else: # 如果在最下面没有找到线
search_top = h - 60 # 在远处找找有没有线（断线）
search_bot = h - 20
mask_image = img_up[:]
mask_image[0:search_top, 0:w] = 0
mask_image[search_bot:h, 0:w] = 0
for i in range(w // 2 - 17):
if P[0] is not None:
break
for j in range(40):
if P[0] is not None:
break
if not mask_image[search_top + j, w // 2 - 15 + i] == 0:
P = [search_top + j, w // 2 - 15 + i]
elif not mask_image[search_top + j, w // 2 - 15 - i] == 0:
P = [search_top + j, w // 2 - 15 - i]
# 得到可识别的颜色数据，进行识别和色差计算
if P[0] is not None: # 如果在最下面找到线
ix = int(P[1])
iy = int(P[0])
cv2.circle(res, (ix, iy), 10, (255, 0, 255), -1)
if cv2.circle:
diff = ix - w / 2
d_diff = diff - last_diff
twist_calculate(diff, d_diff)
else: # 还没找到,朝上次看见线的方向旋转
twist = Twist()
twist.linear.x = 0
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
if up_diff > 0:
twist.angular.z = -0.2
else:
twist.angular.z = +0.2
pub_cmd.publish(twist)
# 将实际图像和二值化图像通过话题发出
img_msg = bridge.cv2_to_imgmsg(res, encoding="bgr8")
img_msg.header.stamp = rospy.Time.now()
result_pub.publish(img_msg)
img_msg = bridge.cv2_to_imgmsg(mask_image, encoding="passthrough")
img_msg.header.stamp = rospy.Time.now()
mask_pub.publish(img_msg)
# 速度处理函数
def twist_calculate(twist_erro, twist_d_erro):
global last_diff, up_diff, diff
twist = Twist()
twist.linear.x = 0.1
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
if abs(twist_erro) < 20: # 有偏转，向重心方向偏转
twist.angular.z = 0
else:
twist.angular.z = -float(twist_erro) * 0.0025 - float(twist_d_erro) * 0.000
up_diff = twist_d_erro
if abs(twist_erro) > 100: # 太大了，停下来慢慢转
twist.linear.x = 0
last_diff = diff
pub_cmd.publish(twist)
if __name__ == '__main__':
try:
# init ROS node
rospy.init_node("line_follow")
rospy.loginfo("Starting Line Follow node")
line_follow_init()
rospy.spin()
except KeyboardInterrupt:
print("Shutting down cv_bridge_test node.")
cv2.destroyAllWindows()
建议通过U盘拷到小车上（建议复制粘贴文本进去，不要复制粘贴文件进去）通过远程传输，远程连接，虚拟机之类的，里面的中文可能会变成乱码，暂不知道对运行有何影响
如果在小车上不能运行，请右键该文件，选择属性，第二栏，勾选可执行选项。
