智能小车倒车入库功能的实现（python,cpp,ubuntu）
From sztu 自动化专业的小菜鸡。
1.基本介绍
倒车入库代码存在于~\config\teleop\src\smartcar\scripts文件目录下的camera_cmd.py中，核心程序位于rear_camera_callback函数中。
程序说明：
rear_camera_callback函数读取后置摄像头的图像数据并据此确定智能车倒车的控制策略。
首先是把原始图像进行畸变校正，结果如下图所示：
从图中可以看出，因为视角以及视野的原因，后置相机很难从复杂的场景中捕捉到车库线，也就很难根据车库线进行倒车。又因为停车标志立放于相机视野中，易于捕捉，且其与车库的位置相对固定，所以本教程利用停车标志进行倒库。
首先是从事业中提取停车标志， findSquare函数可以将视野中的矩形捕捉到，因而可以用来提取停车标志，最终的结果如下图所示：
标志中心如上图红点所示，因为标志中心与车库中轴线有一定距离，所以需要测量两者之间的偏差，偏差约为停车标志宽度的1.05倍。
交通标志中心的位置加上偏差就可以得到车库中轴线的位置，将车库中轴线的位置与智能车中轴线的位置做对比就可以求得偏差，利用该偏差就可以实现对智能车的控制：
上图中黄点为停车标志中心，红点为车库中心，蓝点为智能车中心，在这里我们直接取相机视野的中心。
当智能车逐渐逼近车库时，交通标志在视野中会变得越来越大，直至无法完全捕捉（提取到的矩形宽度w大于某个阈值），此时便无法通过上面的方法进行控制，因为此时智能车部分车身已经入库，所以直接按照之前的位姿进行倒车直至完全入库为止。
停车到位的判断是利用激光测距实现的，当智能车与后方停车标志的距离达到某个阈值时，就认为停车到位了。
这一部分代码存在于~/config/teleop/src/laser_test文件目录下的laster_test.cpp中。
2.代码展示
python部分：
#!/usr/bin/env python
# -*- coding: UTF-8 -*-
import numpy as np
import cv2
import matplotlib.pyplot as plt
from collections import deque
import rospy
from std_msgs.msg import String
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
from ackermann_msgs.msg import AckermannDriveStamped
from laser_test.msg import laser_control
import jetson.inference
import jetson.utils
import argparse
import sys
import math
import os
#from skimage import morphology
import time
global x0
x0 = 1
global peak_thresh
peak_thresh = 50
global n
n = 0
global laser_cmd
laser_cmd = 0
global msg
msg = AckermannDriveStamped()
global flag1
flag1 = 0
global flag2
flag2 = 0
global flag3
flag3 = 0
global c0,c1
intrinsicMat = np.array([[489.3828, 0.8764, 297.5558],
[0, 489.8446, 230.0774],
[0, 0, 1]])
distortionCoe = np.array([-0.4119,0.1709,0,0.0011, 0.018])
src_pts = np.float32([[128,378],[1,435],[639,435],[488,378]])#[220,306],[1,435],[639,435],[451,306] [[132,358],[1,435],[639,435],[501,353] [160,334],[38,378],[628,378],[502,334]
dst_pts = np.float32([[70,0],[70,480],[570,480],[570,0]])
showMe = 0
#net = jetson.inference.imageNet("resnet-18",labels="labels.txt")
def guide_board(data):
#net = jetson.inference.imageNet(opt.network, sys.argv)
#img = CvBridge().imgmsg_to_cv2(data, "bgr8")
image = cv2.cvtColor(data, cv2.COLOR_BGR2HSV)
#blue_lower=np.array([100,50,50])
# blue_upper=np.array([124,255,255])
lower_blue = np.array([100, 50, 50])
upper_blue = np.array([124, 255, 255])
mask = cv2.inRange(image, lower_blue, upper_blue)
imge0 = cv2.bitwise_and(data, data, mask=mask)
cv2.imwrite('123.jpg',imge0)
#data0 = np.array(data)
#data = jetson.utils.cudaMemory(data0)
#img = jetson.utils.cudaCrop(data)
img = jetson.utils.loadImage("123.jpg")
class_idx, confidence = net.Classify(img)
print(class_idx,confidence)
return class_idx,confidence
def display(img,title,color=1):
'''
func:display image
img: rgb or grayscale
title:figure title
color:show image in color(1) or grayscale(0)
'''
if showMe:
if color:
plt.imshow(img)
else:
plt.imshow(img ,cmap='gray')
plt.title(title)
plt.axis('off')
plt.show()
def light_detection(origin_img):
light_cmd=0
hsv=cv2.cvtColor(origin_img,cv2.COLOR_BGR2HSV)
hsv1 = hsv.copy()
element = cv2.getStructuringElement(cv2.MORPH_RECT,(5,5))
red_lower = np.array([0,95,230]) #这两个是阈值，红灯的上界和下界
red_upper = np.array([5,255,255])
red_mask = cv2.inRange(hsv,red_lower,red_upper)
red_target = cv2.bitwise_and(hsv,hsv,mask = red_mask)
red_target = cv2.erode(red_target,element)
red_target = cv2.dilate(red_target,element)
red_gray = cv2.cvtColor(red_target,cv2.COLOR_BGR2GRAY)
r_ret,r_binary = cv2.threshold(red_mask,127,255,cv2.THRESH_BINARY)
r_gray2 = cv2.Canny(r_binary, 100, 200)
r = r_gray2[:,:] == 255
count_red = len(r_gray2[r])
if count_red>700:
redLight = 1
else:
redLight = 0
green_lower = np.array([70,95,230])
green_upper = np.array([130,255,255])
green_mask = cv2.inRange(hsv1,green_lower,green_upper)
green_target = cv2.bitwise_and(hsv1,hsv1,mask = green_mask)
green_target = cv2.erode(green_target,element)
green_target = cv2.dilate(green_target,element)
green_gray = cv2.cvtColor(green_target,cv2.COLOR_BGR2GRAY)
g_ret,g_binary = cv2.threshold(green_mask,127,255,cv2.THRESH_BINARY)
g_gray2 = cv2.Canny(g_binary, 100, 200)
g = g_gray2[:,:] == 255
count_green = len(g_gray2[g])
if count_green>700:
greenLight = 1
else:
greenLight = 0
if redLight ==1 :
light_cmd = 0
if greenLight == 1 :
light_cmd = 1
return light_cmd
def direction_test(image,tem):
#cv.imshow('canny',canny)
#cv.waitKey(3)
undstrt = cv2.undistort(image, intrinsicMat, distortionCoe, None, intrinsicMat)
#masked,x,y,w,h = findSquare(undstrt)
gray = cv2.cvtColor(undstrt, cv2.COLOR_BGR2GRAY)
tem = cv2.cvtColor(tem, cv2.COLOR_BGR2GRAY)
canny = cv2.Canny(gray, 100, 200)
tem_result = cv2.matchTemplate(canny, tem, 4)
#print((left.max(),right.max()))
'''
l = left.max()
r = right.max()
print(max(l,r))
if l<2000000 and r<2000000:
print('null)
if l>r:
print('left')
return -1
else:
print('right')
return 1
'''
# print(tem_result.max())
return tem_result.max()
def turn(img):
list=[0,0,0,0,0]
park_max=0
gostraight_max=0
turnleft_max=0
turnright_max=0
turnback_max=0
for filename in os.listdir(r'/home/nano/config/teleop/src/smartcar/scripts/template/park'):
filenames = '/home/nano/config/teleop/src/smartcar/scripts/template/park/'+ filename
tem = cv2.imread(filenames)
park = direction_test(img,tem)
if park>park_max:
park_max=park
#list[0]=park_max
for filename in os.listdir(r'/home/nano/config/teleop/src/smartcar/scripts/template/gostraight'):
filenames = '/home/nano/config/teleop/src/smartcar/scripts/template/gostraight/'+filename
tem = cv2.imread(filenames)
gostraight = direction_test(img,tem)
if gostraight>gostraight_max:
gostraight_max=gostraight
list[1]=gostraight_max
for filename in os.listdir(r'/home/nano/config/teleop/src/smartcar/scripts/template/turnleft'):
filenames = '/home/nano/config/teleop/src/smartcar/scripts/template/turnleft/'+ filename
tem = cv2.imread(filenames)
turnleft = direction_test(img,tem)
if turnleft>turnleft_max:
turnleft_max=turnleft
list[2]=turnleft_max
for filename in os.listdir(r'/home/nano/config/teleop/src/smartcar/scripts/template/turnright'):
filenames = '/home/nano/config/teleop/src/smartcar/scripts/template/turnright/'+ filename
tem = cv2.imread(filenames)
turnright = direction_test(img,tem)
if turnright>turnright_max:
turnright_max=turnright
list[3]=turnright_max
for filename in os.listdir(r'/home/nano/config/teleop/src/smartcar/scripts/template/turnback'):
filenames = '/home/nano/config/teleop/src/smartcar/scripts/template/turnback/'+ filename
tem = cv2.imread(filenames)
turnback = direction_test(img,tem)
if turnback>turnback_max:
turnback_max=turnback
#list[4]=turnback_max
max = np.max(list) #最大值
print(max)
max_p = np.where(list==max)[0]
#if left<2000000 and right<2000000:
#delay(0.1,27,23)
#print('!!!!')
#print((left,right))
print('park_max',park_max)
print('gostraight_max',gostraight_max)
print('turnleft_max',turnleft_max)
print('turnright_max',turnright_max)
print('turnback_max',turnback_max)
if max_p==0:
print('park')
if max_p==1:
print('gostraight')
if max_p==2:
print('turnleft')
if max_p==3:
print('turnright')
if max_p==4:
print('turnback')
return max_p, park_max,gostraight_max,turnleft_max,turnright_max,turnback_max
def birdView(img,M):
'''
Transform image to birdeye view
img:binary image
M:transformation matrix
return a wraped image
'''
img_sz = (img.shape[1],img.shape[0])
img_warped = cv2.warpPerspective(img,M,img_sz,flags = cv2.INTER_LINEAR)
return img_warped
def perspective_transform(src_pts,dst_pts):
'''
perspective transform
args:source and destiantion points
return M and Minv
'''
M = cv2.getPerspectiveTransform(src_pts,dst_pts)
Minv = cv2.getPerspectiveTransform(dst_pts,src_pts)
return {'M':M,'Minv':Minv}
# original image to bird view (transformation)
def find_centroid(image,peak_thresh,window,showMe):
'''
find centroid in a window using histogram of hotpixels
img:binary image
window with specs {'x0','y0','width','height'}
(x0,y0) coordinates of bottom-left corner of window
return x-position of centroid ,peak intensity and hotpixels_cnt in window
'''
#crop image to window dimension
mask_window = image[int(window['y0']-window['height']):int(window['y0']),
int(window['x0']):int(window['x0']+window['width'])]
histogram = np.sum(mask_window,axis=0)
centroid = np.argmax(histogram)
hotpixels_cnt = np.sum(histogram)
peak_intensity = histogram[centroid]
if peak_intensity<=peak_thresh:
centroid = int(round(window['x0']+window['width']/2))
peak_intensity = 0
else:
centroid = int(round(centroid+window['x0']))
'''
if showMe:
plt.plot(histogram)
plt.title('Histogram')
plt.xlabel('horzontal position')
plt.ylabel('hot pixels count')
plt.show()
'''
return (centroid,peak_intensity,hotpixels_cnt)
def find_starter_centroids(image,y0,peak_thresh,showMe):
'''
find starter centroids using histogram
peak_thresh:if peak intensity is below a threshold use histogram on the full height of the image
returns x-position of centroid and peak intensity
'''
window = {'x0':0,'y0':y0,'width':image.shape[1],'height':image.shape[0]/5}
# get centroid
centroid , peak_intensity,_ = find_centroid(image,peak_thresh,window,showMe)
if peak_intensity image.shape[1]:
window['x0'] = image.shape[1] - sliding_window_specs['width']
centroid, peak_intensity, hotpixels_cnt = find_centroid(image, peak_thresh, window, showMe=0)
if step == 0:
starter_centroid = centroid
if hotpixels_cnt / (window['width'] * window['height']) > 0.6:
window['width'] = window['width'] * 2
window['x0'] = round(window['x0'] - window['width'] / 2)
if (window['x0'] < 0): window['x0'] = 0
if (window['x0'] + window['width']) > image.shape[1]:
window['x0'] = image.shape[1] - window['width']
centroid, peak_intensity, hotpixels_cnt = find_centroid(image, peak_thresh, window, showMe=0)
# if showMe:
# print('peak intensity{}'.format(peak_intensity))
# print('This is centroid:{}'.format(centroid))
mask_window = np.zeros_like(image)
mask_window[int(window['y0'] - window['height']):int(window['y0']), int(window['x0']):int(window['x0'] + window['width'])] = image[int(window['y0'] - window['height']):int(window['y0']), int(window['x0']):int(window['x0'] + window['width'])]
hotpixels = np.nonzero(mask_window)
# print(hotpixels_log['x'])
hotpixels_log['x'].extend(hotpixels[0].tolist())
hotpixels_log['y'].extend(hotpixels[1].tolist())
# update record of centroid
centroids_log.append(centroid)
out_img = cv2.rectangle(image,(int(window['x0']), int(window['y0'] - window['height'])), (int(window['x0'] + window['width']), int(window['y0'])), (255, 0, 0), 2)
'''
if Left_or_Right == 0:
result[0:image.shape[0],0:image.shape[1]] = out_img
if Left_or_Right == 1:
result[0:image.shape[0],image.shape[1]:image.shape[1]*2] = out_img
if showMe:
cv2.rectangle(out_img,
(int(window['x0']), int(window['y0'] - window['height'])),
(int(window['x0'] + window['width']), int(window['y0'])), (0, 255, 0), 2)
if step == 9:
plt.imshow(out_img)
plt.show()
print(window['y0'])
plt.imshow(out_img)
'''
# set next position of window and use standard sliding window width
window['width'] = sliding_window_specs['width']
window['x0'] = round(centroid - window['width'] / 2)
window['y0'] = window['y0'] - window['height']
return hotpixels_log, out_img
def MahalanobisDist(x, y):
'''
Mahalanobis Distance for bi-variate distribution
'''
covariance_xy = np.cov(x, y, rowvar=0)
inv_covariance_xy = np.linalg.inv(covariance_xy)
xy_mean = np.mean(x), np.mean(y)
x_diff = np.array([x_i - xy_mean[0] for x_i in x])
y_diff = np.array([y_i - xy_mean[1] for y_i in y])
diff_xy = np.transpose([x_diff, y_diff])
md = []
for i in range(len(diff_xy)):
md.append(np.sqrt(np.dot(np.dot(np.transpose(diff_xy[i]), inv_covariance_xy), diff_xy[i])))
return md
def MD_removeOutliers(x, y, MD_thresh):
'''
remove pixels outliers using Mahalonobis distance
'''
MD = MahalanobisDist(x, y)
threshold = np.mean(MD) * MD_thresh
nx, ny, outliers = [], [], []
for i in range(len(MD)):
if MD[i] <= threshold:
nx.append(x[i])
ny.append(y[i])
else:
outliers.append(i)
return (nx, ny)
def update_tracker(tracker,new_value):
'''
update tracker(self.bestfit or self.bestfit_real or radO Curv or hotpixels) with new coeffs
new_coeffs is of the form {'a2':[val2,...],'a1':[va'1,...],'a0':[val0,...]}
tracker is of the form {'a2':[val2,...]}
update tracker of radius of curvature
update allx and ally with hotpixels coordinates from last sliding window
'''
allx = []
ally = []
if tracker =='bestfit':
bestfit['a0'].append(new_value['a0'])
bestfit['a1'].append(new_value['a1'])
bestfit['a2'].append(new_value['a2'])
elif tracker == 'bestfit_real':
bestfit_real['a0'].append(new_value['a0'])
bestfit_real['a1'].append(new_value['a1'])
bestfit_real['a2'].append(new_value['a2'])
elif tracker == 'radOfCurvature':
radOfCurv_tracker.append(new_value)
elif tracker == 'hotpixels':
allx.append(new_value['x'])
ally.append(new_value['y'])
def polynomial_fit(data):
'''
多项式拟合
a0+a1 x+a2 x**2
data:dictionary with x and y values{'x':[],'y':[]}
'''
a2,a1,a0 = np.polyfit(data['x'],data['y'],2)
return {'a0':a0,'a1':a1,'a2':a2}
def predict_line(x0,xmax,coeffs):
'''
predict road line using polyfit cofficient
x vaues are in range (x0,xmax)
polyfit coeffs:{'a2':,'a1':,'a2':}
returns array of [x,y] predicted points ,x along image vertical / y along image horizontal direction
'''
x_pts = np.linspace(x0,xmax-1,num=xmax)
pred = coeffs['a2']*x_pts**2+coeffs['a1']*x_pts+coeffs['a0']
return np.column_stack((x_pts,pred))
class PID:
def __init__(self, P=0.2, I=0.0, D=0.0):
self.Kp = P
self.Ki = I
self.Kd = D
self.sample_time = 0.00
self.current_time = time.time()
self.last_time = self.current_time
self.clear()
def clear(self):
self.SetPoint = 0.0
self.PTerm = 0.0
self.ITerm = 0.0
self.DTerm = 0.0
self.last_error = 0.0
# Windup Guard
self.int_error = 0.0
self.windup_guard = 20.0
self.output = 0.0
def update(self, feedback_value):
error = self.SetPoint - feedback_value
self.current_time = time.time()
delta_time = self.current_time - self.last_time
delta_error = error - self.last_error
if (delta_time >= self.sample_time):
self.PTerm = self.Kp * error
self.ITerm += error * delta_time
if (self.ITerm < -self.windup_guard):
self.ITerm = -self.windup_guard
elif (self.ITerm > self.windup_guard):
self.ITerm = self.windup_guard
self.DTerm = 0.0
if delta_time > 0:
self.DTerm = delta_error / delta_time
self.last_time = self.current_time
self.last_error = error
self.output = self.PTerm + (self.Ki * self.ITerm) + (self.Kd * self.DTerm)
def setKp(self, proportional_gain):
self.Kp = proportional_gain
def setKi(self, integral_gain):
self.Ki = integral_gain
def setKd(self, derivative_gain):
self.Kd = derivative_gain
def setWindup(self, windup):
self.windup_guard = windup
def setSampleTime(self, sample_time):
self.sample_time = sample_time
def compute_radOfCurvature(coeffs,pt):
return ((1+(2*coeffs['a2']*pt+coeffs['a1'])**2)**1.5)/np.absolute(2*coeffs['a2'])
def binarize(img):
"""Binarize a grayscale image.
Binarize the input grayscale image by ostu threshold method.
Args:
img: an image. Grayscale image is preffered.
Returns:
img_binary: the binarized image
"""
# Make sure that img_gray is a grayscale image.
if len(img.shape) == 2:
img_gray = img
elif len(img.shape) == 3 and img.shape[2] == 3:
img_gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
else:
#print("Converting image failed:", img.shape)
return None
# Apply the threshold method. It can be improved by changing the arguments.
_, img_binary = cv2.threshold(
img_gray, 170, 255, cv.THRESH_OTSU)
return img_binary
def lane_detection(img):
start_time=time.time()
corr_img = cv2.undistort(img, intrinsicMat, distortionCoe, None, intrinsicMat)
#cv2.imwrite('000.jpg',corr_img)
cv2.imshow('corr_img',corr_img)
cv2.waitKey(25)
gray_ex = cv2.cvtColor(corr_img,cv2.COLOR_RGB2GRAY)
display(gray_ex,'Apply Camera Correction',color=0)
#ret, combined_output = cv2.threshold(gray_ex, 0, 255, cv2.THRESH_BINARY | cv2.THRESH_OTSU)
combined_output = cv2.Canny(gray_ex, 400, 800) #100, 200 75,200
#cv2.imwrite('001.jpg',combined_output)
#combined_output = image_process(gray_ex)
display(combined_output,'Combined output',color=0)
#mask = np.zeros_like(combined_output)
#vertices = np.array([[(300,278),(0,435),(640,435),(450,250)]],dtype=np.int32)
#cv2.fillPoly(mask,vertices,1)
#masked_image = cv2.bitwise_and(combined_output,mask)
#display(masked_image,'Masked',color=0)
min_sz = 50
#cleaned = morphology.remove_small_objects(masked_image.astype('bool'),min_size=min_sz,connectivity=2)
cleaned = combined_output
display(cleaned,'cleaned',color=0)
# original image to bird view (transformation)
transform_matrix = perspective_transform(src_pts,dst_pts)
warped_image = birdView(cleaned*1.0,transform_matrix['M'])
warped_image = cv2.dilate(warped_image, np.ones((15,15), np.uint8))
warped_image = cv2.erode(warped_image, np.ones((7,7), np.uint8))
#cv2.imwrite('002.jpg',warped_image)
#pubbrid_view.publish(CvBridge().cv2_to_imgmsg(warped_image))
display(cleaned,'undistorted',color=0)
display(warped_image,'BirdViews',color=0)
#white_Left = cv2.countNonZero(warped_image[:,0:warped_image.shape[1]/2-50])
#white_Right = cv2.countNonZero(warped_image[:,warped_image.shape[1]/2+50:warped_image.shape[1]])
######mid_time
mid_time=time.time()
HoughLine_image = np.array(warped_image,np.uint8)
lines = cv2.HoughLinesP(HoughLine_image,1,np.pi/180,100,100,100,50)
if lines is not None :
for x1,y1,x2,y2 in lines[0]:
cv2.line(HoughLine_image,(x1,y1),(x2,y2),(255,0,0),1)
########################################################################fit##################################################################################
bottom_crop = -40
#warped_image = warped_image[0:bottom_crop,:]
peak_thresh = 10
showMe = 1
centroid_starter_top = find_starter_centroids(warped_image,y0=warped_image.shape[0],
peak_thresh=peak_thresh,showMe=showMe)
centroid_starter_bottom = find_starter_centroids(warped_image,y0=warped_image.shape[0]/5,
peak_thresh=peak_thresh,showMe=showMe)
print('centroid_starter_TOP',centroid_starter_top['centroid'])
print('centroid_starter_BOTTOM',centroid_starter_bottom['centroid'])
'''
centroid_starter_right = find_starter_centroids(warped_image,x0=warped_image.shape[1]/2,
peak_thresh=peak_thresh,showMe=showMe)
centroid_starter_left = find_starter_centroids(warped_image,x0=0,peak_thresh=peak_thresh,
showMe=showMe)
sliding_window_specs = {'width': 60, 'n_steps': 10}
print('centroid_starter_right',centroid_starter_right)
#cv2.imshow('warped_image',warped_image)
#cv2.waitKey(25)
####window#####
'''
MD_thresh = 1.8
#log_lineLeft['x'], log_lineLeft['y'] = \
#MD_removeOutliers(log_lineLeft['x'], log_lineLeft['y'], MD_thresh)
#log_lineRight['x'], log_lineRight['y'] = \
#MD_removeOutliers(log_lineRight['x'], log_lineRight['y'], MD_thresh)
ym_per_pix = 0.6/480
xm_per_pix = 0.6/640
###log_lineLeft,out_img_Left = run_sliding_window(warped_image, centroid_starter_left['centroid'],sliding_window_specs, showMe=showMe)
###log_lineRight,out_img = run_sliding_window(out_img_Left, centroid_starter_right['centroid'] , sliding_window_specs,showMe=showMe)
global c0,c1 ##################################
if lines is not None :
for i in range(warped_image.shape[1]/5):
x = 5*i
px = [x1,x2]
py = [y1,y2]
c1,c0 = np.polyfit(px, py, 1)
y = int(c1*x + c0)
cv2.circle(warped_image, (x, y), 3, (255, 0, 0), -1)
print('HoughLine is detected')
print('c1',c1)
cv2.imshow('warped_image',warped_image)
cv2.waitKey(25)
'''
for i in range(len(log_lineRight['y'])):
log_lineRight['y'][i] = log_lineRight['y'][i] + out_img.shape[1]/2
#log_lineRight['x'] = log_lineRight['x'] + out_img.shape[1]/2
fit_lineRight_singleframe = polynomial_fit(log_lineRight)
fit_lineLeft_singleframe = polynomial_fit(log_lineLeft)
dis_Left = log_lineLeft['y'][len(log_lineLeft['y'])-1] - log_lineLeft['y'][0]
dis_Right = log_lineRight['y'][len(log_lineRight['y'])-1] - log_lineRight['y'][0]
var_pts = np.linspace(0,corr_img.shape[0]-1,num=corr_img.shape[0])
pred_lineLeft_singleframe = predict_line(0,corr_img.shape[0],fit_lineLeft_singleframe)
fit_lineLeft_real = polynomial_fit({'x':[i*xm_per_pix for i in log_lineLeft['x']],
'y':[i*ym_per_pix for i in log_lineLeft['y']]})
pred_lineRight_sigleframe = predict_line(0,corr_img.shape[0],fit_lineRight_singleframe)
fit_lineRight_real = polynomial_fit({'x':[i*xm_per_pix for i in log_lineRight['x']],
'y':[i*ym_per_pix for i in log_lineRight['y']]})
pt_curvature = corr_img.shape[0]
radOfCurv_r = compute_radOfCurvature(fit_lineRight_real,pt_curvature*ym_per_pix)
radOfCurv_l = compute_radOfCurvature(fit_lineLeft_real,pt_curvature*ym_per_pix)
average_radCurv = (radOfCurv_r+radOfCurv_l)/2
center_of_lane = (pred_lineLeft_singleframe[:,1][-1]+pred_lineRight_sigleframe[:,1][-1])/2
offset = (corr_img.shape[1]/2 - center_of_lane)*xm_per_pix
side_pos = 'right'
if offset <0:
side_pos = 'left'
wrap_zero = np.zeros_like(gray_ex).astype(np.uint8)
color_wrap = np.dstack((wrap_zero,wrap_zero,wrap_zero))
left_fitx = fit_lineLeft_singleframe['a2']*var_pts**2 + fit_lineLeft_singleframe['a1']*var_pts + fit_lineLeft_singleframe['a0']
right_fitx = fit_lineRight_singleframe['a2']*var_pts**2 + fit_lineRight_singleframe['a1']*var_pts+fit_lineRight_singleframe['a0']
pts_left = np.array([np.transpose(np.vstack([left_fitx,var_pts]))])
pts_right = np.array([np.flipud(np.transpose(np.vstack([right_fitx,var_pts])))])
pts = np.hstack((pts_left,pts_right))
cv2.fillPoly(color_wrap,np.int_([pts]),(0,255,0))
cv2.putText(color_wrap,'|',(int(corr_img.shape[1]/2),corr_img.shape[0]-10),cv2.FONT_HERSHEY_SIMPLEX,2,(0,0,255),8)
cv2.putText(color_wrap,'|',(int(center_of_lane),corr_img.shape[0]-10),cv2.FONT_HERSHEY_SIMPLEX,1,(255,0,0),8)
newwrap = cv2.warpPerspective(color_wrap,transform_matrix['Minv'],(corr_img.shape[1],corr_img.shape[0]))
result = cv2.addWeighted(corr_img,1,newwrap,0.3,0)
cv2.putText(result,'Vehicle is ' + str(round(offset,3))+'m '+side_pos+' of center',
(50,100),cv2.FONT_HERSHEY_SIMPLEX,1,(255,255,255),thickness=2)
cv2.putText(result,'Radius of curvature: '+str(round(average_radCurv,3))+'m',(50,50),cv2.FONT_HERSHEY_SIMPLEX,1,(255,255,255),thickness=2)
# cv2.imshow("result",result)
#cv2.waitKey(25)
'''
end_time=time.time()
time_diff1=mid_time-start_time
time_diff2=end_time-mid_time
print('time_diff1',time_diff1)
print('time_diff2',time_diff2)
#print('offset ', offset)
msg.drive.speed = 20
#print('dis_Left',dis_Left)
#print('dis_Right',dis_Right)
if math.fabs(centroid_starter_top['centroid']-centroid_starter_bottom['centroid'])<80:
##((dis_Left < 90)&(dis_Left > 30) & (dis_Right > -10 )&(dis_Right < 70 )) :#四个dis的阈值都需要调参
print('both of the lanes are detected')
offset=centroid_starter_top['centroid']-centroid_starter_bottom['centroid']
Vehicle_PID.update(offset)
msg.drive.steering_angle = Vehicle_PID.output###正负未测试
else :
print('only single lane is detected')
k1 = 20#50 45# k1,k2 需要调参
k2 = 0.02#0.02 0.035
#####need to adjust para####
if (480-c0)/c1<320:
msg.drive.steering_angle = 1/c1*k1 - k2*(480- c0)/c1
print('Left Lane')
if (480-c0)/c1>320:
msg.drive.steering_angle = 1/c1*k1 + k2*(640-(480-c0)/c1)
print('Right Lane')
print('steering_angle',msg.drive.steering_angle)
def blue_pixel_detection(img):
cross_verify = 0
corr_img = cv2.undistort(img, intrinsicMat, distortionCoe, None, intrinsicMat)
hsv=cv2.cvtColor(corr_img,cv2.COLOR_BGR2HSV)
#cv2.imshow('hsv',hsv)
#提取蓝色区域
blue_lower=np.array([100,50,50])
blue_upper=np.array([124,255,255])
blue_mask=cv2.inRange(hsv,blue_lower,blue_upper)
#模糊
blue_blurred=cv2.blur(blue_mask,(9,9))
#cv2.imshow('blurred',blurred)
#二值化
ret,binary=cv2.threshold(blue_blurred,127,255,cv2.THRESH_BINARY)
transform_matrix = perspective_transform(src_pts,dst_pts)
blue_warped_image = birdView(binary*1.0,transform_matrix['M'])
cv2.imshow('blurred binary',blue_warped_image)
cv2.waitKey(25)
blue_point = cv2.countNonZero(blue_warped_image)
return blue_point
def traffic_sign_detection(img):
global flag1
global flag2
global sign
global park_max
global gostraight_max
global turnleft_max
global turnright_max
global turnback_max
global time1
threshold = 80000000############threshold 需要调参
blue_point = blue_pixel_detection(img)
print('The number of blue pixels',blue_point)
if (blue_point < threshold) & (flag1 == 0):
lane_detection(img)
else:
flag1 = 1
print('Parking line is detected')
if (flag2 == 0):
#######speed=0
msg.drive.speed = 0
pub.publish(msg)
time.sleep(1)
#sign,park_max,gostraight_max,turnleft_max,turnright_max,turnback_max = turn(img)
sign,_ =guide_board(img)
flag2 = 1
time1 = time.time()
else:
if(sign == 0):
msg.drive.steering_angle = 0
msg.drive.speed = 20
if(sign == 1):
msg.drive.steering_angle = 10#以下四个角度都需要调参
msg.drive.speed = 20
if(sign == 2):
msg.drive.steering_angle = -10
msg.drive.speed = 20
if(sign == 3):
msg.drive.steering_angle = 0
msg.drive.speed = 00
if(sign == 4):
msg.drive.steering_angle = 11
msg.drive.speed = 20
time2 = time.time()
if (time2 - time1>3) & (blue_point > threshold):
flag1 = 0
#print('park_max',park_max)
#print('gostraight_max',gostraight_max)
#print('turnleft_max',turnleft_max)
#print('turnright_max',turnright_max)
#print('turnback_max',turnback_max)
print('The speed is ', msg.drive.speed)
print('The steering angle is ', msg.drive.steering_angle)
pub.publish(msg)
def front_camera_callback(data):
#class_idx, confidence = guide_board(data)
#print(class_idx,confidence)
time1=time.time()
img = CvBridge().imgmsg_to_cv2(data, "bgr8")
global n
print(laser_cmd)
if laser_cmd == 0:
if(n>=1):
traffic_sign_detection(img)
else:
if(1):#light_detection(img)
n=n+1
else:
msg.drive.speed = 0
pub.publish(msg)
n = n
#lane_detection(img)
time2=time.time()
print('totaltime',time2-time1)
def laser_callback(data):
global laser_cmd
laser_cmd = data.laser_control
def findSquare( image ):
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
blurred = cv2.GaussianBlur(gray, (7, 7), 0)
edged = cv2.Canny(blurred, 60, 120)
cv2.imshow("edged",edged )
cv2.waitKey(25)
# find contours in the edge map
(cnts, _) = cv2.findContours(edged.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
# loop over our contours to find hexagon
cnts = sorted(cnts, key = cv2.contourArea, reverse = True)[:50]
screenCnt = None
for c in cnts:
# approximate the contour
peri = cv2.arcLength(c, True)
approx = cv2.approxPolyDP(c, 0.004 * peri, True)
# if our approximated contour has four points, then
# we can assume that we have found our squeare
if len(approx) >= 4:
screenCnt = approx
x,y,w,h = cv2.boundingRect(c)
cv2.drawContours(image, [approx], -1, (0, 0, 255), 1)
#cv2.imshow("Screen", image)
#create the mask and remove rest of the background
mask = np.zeros(image.shape[:2], dtype = "uint8")
cv2.drawContours(mask, [screenCnt], -1, 255, -1)
masked = cv2.bitwise_and(image, image, mask = mask)
cv2.circle(masked, (int(x+w/2), int(y+h/2)), 3, (0, 0, 255), -1)
cv2.imshow("Masked",masked )
cv2.waitKey(25)
#cv2.imwrite('004.jpg',masked)
#crop the masked image to to be compared to referance image
#cropped = masked[y:y+h,x:x+w]
#scale the image so it is fixed size as referance image
#cropped = cv2.resize(cropped, (200,200), interpolation =cv2.INTER_AREA)
return masked,x,y,w,h
def rear_camera_callback(data):
global flag3
img = CvBridge().imgmsg_to_cv2(data, "bgr8")
corr_img = cv2.undistort(img, intrinsicMat, distortionCoe, None, intrinsicMat)
#cv2.imwrite('005.jpg',corr_img)
down_img = corr_img[:, corr_img.shape[1]/2:corr_img.shape[1]]
masked,x,y,w,h = findSquare(down_img)
#k1,k2,k3,k4,k5,k6=turn(img)
cv2.waitKey(25)
dis = int(-1.05*w)##dis需要调参
cv2.circle(corr_img, (int(x+w/2+corr_img.shape[1]/2), int(y+h/2)), 3, (0, 255, 255), -1)
cv2.circle(corr_img, (int(x+w/2+dis+corr_img.shape[1]/2), int(y+h/2)), 3, (0, 0, 255), -1)
cv2.circle(corr_img, (corr_img.shape[1]/2, int(y+h/2)), 3, (255, 0, 0), -1)
cv2.imshow('result',corr_img)
#cv2.imwrite('003.jpg',corr_img)
print('w= ',w,'h= ',h)
if laser_cmd == 0:
msg.drive.speed = -20
if (w <180) & (flag3 == 0):#w<180需要调参
offset = - (dis + x+w/2)
print('offset',offset)
Reverse_PID.update(offset)
msg.drive.steering_angle = Reverse_PID.output
else:
msg.drive.steering_angle = 0
flag3 = 1
'''
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
edged = cv2.Canny(gray, 400, 800)
transform_matrix = perspective_transform(src_pts,dst_pts)
warped_image = birdView(edged*1.0,transform_matrix['M'])
element1 = cv2.getStructuringElement(0,(2,80))
element2 = cv2.getStructuringElement(0,(2,2))
eroded = cv2.erode(warped_image,element1)
dilated = cv2.dilate(warped_image,element2)
HoughLine_image = np.array(dilated,np.uint8)
lines = cv2.HoughLinesP(HoughLine_image,1,np.pi/180,100,100,100,50)
if lines is not None :
for x1,y1,x2,y2 in lines[0]:
cv2.line(HoughLine_image,(x1,y1),(x2,y2),(255,0,0),1)
for i in range(HoughLine_image.shape[1]/5):
x = 5*i
px = [x1,x2]
py = [y1,y2]
c1,c0 = np.polyfit(px, py, 1)
y = int(c1*x + c0)
cv2.circle(HoughLine_image, (x, y), 3, (255, 0, 0), -1)
cv2.imshow("HoughLine_image",HoughLine_image )
cv2.waitKey(25)
k1 = 55
k2 = 0.02
if (480-c0)/c1<320:
msg.drive.steering_angle = - 1/c1*k1 + k2*(480- c0)/c1
if (480-c0)/c1>320:
msg.drive.steering_angle = - 1/c1*k1 - k2*(640-(480-c0)/c1)
'''
else :
msg.drive.speed = 0
msg.drive.steering_angle = 0
flag3 = 0
print('The speed is ',msg.drive.speed)
print('The steering angle is ',msg.drive.steering_angle)
pub.publish(msg)
def detector():
global pub
global pubresult
global Vehicle_PID
global Reverse_PID
Vehicle_PID = PID(0.05,0,0)#PID需要调参(2,0,0)
Reverse_PID = PID(0.25,0,0)#PID需要调参
rospy.init_node('camera_cmd', anonymous=False)
#rospy.Subscriber("/usb_cam_1/image", Image, front_camera_callback, queue_size=1, buff_size=2**24)
rospy.Subscriber("/usb_cam_1/image", Image, rear_camera_callback, queue_size=1, buff_size=2**24)
rospy.Subscriber("/laser_control", laser_control, laser_callback, queue_size=1)
pub = rospy.Publisher('/ackermann_cmd', AckermannDriveStamped, queue_size=1)
rospy.spin()
if __name__ == '__main__':
parser = argparse.ArgumentParser(description="Classify a live camera stream using an image recognition DNN.",
formatter_class=argparse.RawTextHelpFormatter, epilog=jetson.inference.imageNet.Usage() +
jetson.utils.videoSource.Usage() + jetson.utils.videoOutput.Usage() + jetson.utils.logUsage())
parser.add_argument("input_URI", type=str, default="", nargs='?', help="URI of the input stream")
parser.add_argument("output_URI", type=str, default="", nargs='?', help="URI of the output stream")
parser.add_argument("--network", type=str, default="googlenet", help="pre-trained model to load (see below for options)")
parser.add_argument("--camera", type=str, default="0", help="index of the MIPI CSI camera to use (e.g. CSI camera 0)\nor for VL42 cameras, the /dev/video device to use.\nby default, MIPI CSI camera 0 will be used.")
parser.add_argument("--width", type=int, default=1280, help="desired width of camera stream (default is 1280 pixels)")
parser.add_argument("--height", type=int, default=720, help="desired height of camera stream (default is 720 pixels)")
parser.add_argument('--headless', action='store_true', default=(), help="run without display")
is_headless = ["--headless"] if sys.argv[0].find('console.py') != -1 else [""]
try:
opt = parser.parse_known_args()[0]
except:
print("")
parser.print_help()
sys.exit(0)
# load the recognition network
net = jetson.inference.imageNet(opt.network, sys.argv)
detector() cpp部分：
#include
#include
#include
#include
#include
#include
#include
#include
#include "opencv2/opencv.hpp"
#include
#include
#include
#include
#include
using namespace std;
using namespace cv;
float nearest_x,nearest_y,nearest_dist;
ackermann_msgs::AckermannDriveStamped ackermann_cmd;
float k = 0;
float nearest_angle;
float midx,midy,sx=0,sy=0;
laser_test::laser_control laser_control;
laser_geometry::LaserProjection projector_;
sensor_msgs::PointCloud2 cloud;
int num = 1440;//num为雷达扫描一周的点数
//int low_range = num/16,up_range = num*7/16;
int low_range = 540,up_range = 900;
//int low_range = 1,up_range2 = 120;
int flag = 1;
void LaserOP(const sensor_msgs::LaserScan& laser)
{
ackermann_cmd.drive.speed = 20;
tf::TransformListener tfListener_;
projector_.transformLaserScanToPointCloud("laser_link", laser, cloud, tfListener_);
int i,tt=0;
int l=100,r=100;
float xba=0,yba=0,px1,py1,px2,py2,similarity=-1,d1=0,d2=0;
nearest_x=0; nearest_y=100; nearest_dist=100;
for (i=low_range;i=0.0 && py<=1.2
{
tt++;
if (tt==3)
{
nearest_dist=laser.ranges[i]; nearest_x=px; nearest_y=py; tt=0; l=i; r=i;
nearest_angle = laser.angle_min+i*laser.angle_increment;
}
}
else
tt=0;
}
tt=0;
ROS_INFO_STREAM("The nearest distance is: "<0.5 && flag == 0 ) //阈值需要调参
{
ackermann_cmd.drive.steering_angle = 22;//左转弯 角度需要调参
ROS_INFO_STREAM(ackermann_cmd.drive.steering_angle);
}
if(nearest_angle/CV_PI*180>75&&nearest_angle/CV_PI*180<285)// 阈值需要调参
{
ROS_INFO_STREAM(nearest_angle/CV_PI*180);
ackermann_cmd.drive.steering_angle = -22;//右转弯 角度需要调参
ROS_INFO_STREAM(ackermann_cmd.drive.steering_angle);
}
}
else
{
flag = 1;
laser_control.laser_control = 0;
ROS_INFO_STREAM(ackermann_cmd.drive.steering_angle);
low_range = 540;
up_range = 900;
}
}
/*void LaserOP(const sensor_msgs::LaserScan& laser) // This is used for the rear camera
{
//ackermann_cmd.drive.speed = 100;
tf::TransformListener tfListener_;
projector_.transformLaserScanToPointCloud("laser_link", laser, cloud, tfListener_);
int i,tt=0;
int l=100,r=100;
float xba=0,yba=0,px1,py1,px2,py2,similarity=-1,d1=0,d2=0;
nearest_x=0; nearest_y=100; nearest_dist=100;
for (i = 900;i < 1260;i=i+4)
{
float ang=laser.angle_min+i*laser.angle_increment;
float px=-sin(ang)*laser.ranges[i];
float py= cos(ang)*laser.ranges[i];
if (laser.ranges[i]=0.0 && py<=1.2
{
tt++;
if (tt==3)
{
nearest_dist=laser.ranges[i]; nearest_x=px; nearest_y=py; tt=0; l=i; r=i;
nearest_angle = laser.angle_min+i*laser.angle_increment;
}
}
else
tt=0;
}
ROS_INFO_STREAM("The nearest distance is: "<("/ackermann_cmd",1);
ros::Publisher pub_laser_control=nh.advertise("/laser_control",1);
ros::Publisher pub_cloud = nh.advertise("/PointCloud", 1);
ros::Rate rate(40);
while (ros::ok())
{
ros::spinOnce();
//pub_vel.publish(ackermann_cmd);
pub_cloud.publish(cloud);
pub_laser_control.publish(laser_control);
rate.sleep();
}
}