import math
import socket
import cv2
import numpy as np
import time
from PIL import Image
from unet import Unet_ONNX, Unet
import io
from map_1_23 import *
# q = [[(490,300),(180,300),ip2],[(180,300),(180,89),ip2]]
# 设备信息
local_ip = '100.117.246.71' # 云服务器的IP地址
local_port = 11235 # 云服务器的端口号
username = "admin" # 摄像头管理员账号
password = "lxf666lxf" # 摄像头管理员密码
# ip = ["ip地址",
# [
# [(4个点的透视像素坐标)],
# [(4个点的正视像素坐标)],
# (图像的长, 图像的高)
# ],
# [正视像素坐标转为世界坐标的参数]
# ]
# ip1 = ["10.244.0.203",
# [
# [(1738, 345), (424, 110), (1695, 123), (570, 1019)],
# [(452 * 2, 256 * 2), (52 * 2, 128 * 2), (490 * 2, 128 * 2), (161 * 2, 455 * 2)],
# (637 * 2, 544 * 2)
# ],
# [0.5, 0, -0.5, 544]
# ]
# ip2 = ["10.244.0.202",
# [
# [(566, 68), (1429, 225), (537, 427), (316, 742)],
# [(185 * 2, 0 * 2), (637 * 2, 0 * 2), (185 * 2, 288 * 2), (147 * 2, 416 * 2)],
# (637 * 2, 544 * 2)
# ],
# [-0.5, 637, 0.5, 0]
# ]
ip1 = ["10.244.5.203",
[
[(1738, 345), (424, 110), (1695, 123), (570, 1019)],
[(710, 376), (82, 188), (769, 188), (253, 669)],
(500 * 2, 400 * 2)
],
[0.5, 0, 0.5, 0] # x2 = 0.5 * y1;y2 = 0.5 * x1
]
ip2 = ["10.244.5.202",
[
[(566, 68), (1429, 225), (537, 427), (316, 742)],
[(291, 0), (1000, 0), (291, 424), (231, 612)],
(500 * 2, 400 * 2)
],
[-0.5, 400, -0.5, 500] # x2 = 400 - 0.5 * y1;y2 = 500 - 0.5 * x1
]
unet = Unet()
# 信号发送
'''
输入为控制车辆运动的指令信号
拖车指令:
一级指令(基础指令,每条指令只传递一个数据至一个控制器)
轮子角度-转动速度(v1+speed[3位/4位]),
顺时针转动(clock),
逆时针转动(unclock),
停止转动(stall),
重启控制器 (resetturn)
轮子前进-转动速度(v2+speed[3位/4位]),
前进(forward),
后退(backward),
停车(stop),
重启控制器(resetrun)
叉子升降-上升(up),
悬停(hold),
下降(down)
二级指令(在基础指令的基础上,进行多指令传递)
轮子角度-顺时针转90°(clock90),
逆时针转90°(unclock90),
顺时针转xx°(shun+speed[4位]+angle[2位]),
逆时针转xx°(nnii+speed[4位]+angle[2位])
车身转弯-左转90度(left90run),
右转90度(right90run),
车身向左微调(adjustleft),
车身向右微调(adjustright) 转速与负载有关
装卸货物-装载上升(upload),
卸载下降(download)
'''
def Input(data):
HOST = local_ip
PORT = local_port
BUFSIZ = 1024
ADDR = (HOST, PORT) # 服务器的ADDR
tcpCliSock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
tcpCliSock.connect(ADDR) # 连接到服务器
data1 = data.encode('utf-8')
tcpCliSock.send(data1)
data1 = tcpCliSock.recv(BUFSIZ)
# print("a:", a - 1)
print(data)
tcpCliSock.close()
# 获取原始图像
def picture(ip):
url = f"http://{username}:{password}@{ip}/ISAPI/Streaming/channels/1/picture"
capture = cv2.VideoCapture(url)
if not capture.isOpened():
print(f"无法连接摄像头 {ip}")
ret, frame = capture.read()
if ret:
# 在此处进行图像处理或保存操作
frame1 = cv2.resize(frame, (1920, 1080))
else:
print(f"无法从摄像头 {ip} 读取图像")
capture.release()
return frame1
# 去畸形
def undistort(img):
img = cv2.resize(img, (1920, 1080))
map1, map2 = cv2.fisheye.initUndistortRectifyMap(np.array(
[[1338.0486445974152, 0.0, 975.2400726485404], [0.0, 1350.3644432497836, 527.4095409601543],
[0.0, 0.0, 1.0]]),
np.array([[0.10263776481979028], [-0.5389050149517198],
[0.43418969823042103], [0.39410326833200016]]),
np.eye(3), np.array(
[[1338.0486445974152, 0.0, 975.2400726485404], [0.0, 1350.3644432497836, 527.4095409601543],
[0.0, 0.0, 1.0]]), (1920, 1080), cv2.CV_16SC2)
undistorted_img = cv2.remap(img, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT)
return undistorted_img
# 坐标转换-逆透视转换
def coord_transform(img, ip_coord):
pts1 = np.float32(ip_coord[0]) # 原图4个点坐标
pts2 = np.float32(ip_coord[1]) # 透视图4个点坐标
matrix = cv2.getPerspectiveTransform(pts1, pts2) # 仿射矩阵
result = cv2.warpPerspective(img, matrix, ip_coord[2]) # ip_coord[2]:修正图像大小(w,h)
return result
# 识别车辆
def predict(img):
img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
img = Image.fromarray(img)
# 使用 BytesIO 在内存中保存图像
buffer = io.BytesIO()
img.save(buffer, format="JPEG")
buffer.seek(0) # 将缓冲区的指针移动到开始位置
# 通过 PIL 显示图像
img_from_buffer = Image.open(buffer)
# print(type(img))
r_image = unet.detect_image(img_from_buffer, count=False, name_classes=["background", "sign"])
img = cv2.cvtColor(np.array(r_image), cv2.COLOR_RGB2BGR)
return img
def left_right_coord(image):
hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
lower_red1 = np.array([0, 120, 70])
upper_red1 = np.array([10, 255, 255])
lower_red2 = np.array([170, 120, 70])
upper_red2 = np.array([180, 255, 255])
mask1 = cv2.inRange(hsv, lower_red1, upper_red1)
mask2 = cv2.inRange(hsv, lower_red2, upper_red2)
mask = cv2.bitwise_or(mask1, mask2)
contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
max_contour = max(contours, key=cv2.contourArea)
rect = cv2.minAreaRect(max_contour)
box = cv2.boxPoints(rect)
box = np.intp(box)
cv2.drawContours(image, [box], 0, (0, 255, 0), 2)
length1 = (box[0][0] - box[1][0]) ** 2 + (box[0][1] - box[1][1]) ** 2
length2 = (box[1][0] - box[2][0]) ** 2 + (box[1][1] - box[2][1]) ** 2
if length1 > length2:
l1 = np.array([[box[0][0], box[0][1]], [box[1][0], box[1][1]]])
l2 = np.array([[box[2][0], box[2][1]], [box[3][0], box[3][1]]])
else:
l1 = np.array([[box[1][0], box[1][1]], [box[2][0], box[2][1]]])
l2 = np.array([[box[3][0], box[3][1]], [box[0][0], box[0][1]]])
x = int(
((2 / 7) * l2[1][0] + (5 / 7) * l1[0][0] + l1[0][0] + (2 / 7) * l2[0][0] + (5 / 7) * l1[1][0] + l1[1][
0]) // 4)
y = int(
((2 / 7) * l2[1][1] + (5 / 7) * l1[0][1] + l1[0][1] + (2 / 7) * l2[0][1] + (5 / 7) * l1[1][1] + l1[1][
1]) // 4)
hsv_value = cv2.cvtColor(np.uint8([[image[y, x]]]), cv2.COLOR_BGR2HSV)
mask3 = cv2.inRange(hsv_value, lower_red1, upper_red1)
mask4 = cv2.inRange(hsv_value, lower_red2, upper_red2)
if mask3 or mask4:
left = l1[1]
right = l1[0]
else:
left = l2[1]
right = l2[0]
return left, right, image
# 像素坐标转世界坐标
def pixel2world(point, ip_pixel2world):
x = int(point[0] * ip_pixel2world[0] + ip_pixel2world[1])
y = int(point[1] * ip_pixel2world[2] + ip_pixel2world[3])
return [x, y]
def pixel2map(point, ip_pixel2world):
x = int(point[1] * ip_pixel2world[0] + ip_pixel2world[1])
y = int(point[0] * ip_pixel2world[2] + ip_pixel2world[3])
return [x, y]
# 输出坐标
def Coord_print(ip):
# 1.获取原始图像
img = picture(ip[0])
# cv2.imwrite("%d_001.jpg" % a,img)
# 2.去畸形
img = undistort(img)
cv2.imwrite("%d_002.jpg" % a, img)
# 3.坐标转换-逆透视转换
img = coord_transform(img, ip[1])
cv2.imwrite("%d_003.jpg" % a, img)
# 4.识别车辆
img = predict(img)
left, right, img = left_right_coord(img)
cv2.imwrite("%d_004.jpg" % a, img)
# 5.像素坐标转世界坐标
# left = pixel2world(left, ip[2])
# right = pixel2world(right, ip[2])
left = pixel2map(left, ip[2])
right = pixel2map(right, ip[2])
# 6.拖车前后中点世界坐标
backward, forward = MidPoint(left, right)
print('a:', a)
print("车后坐标:", backward)
print("车前坐标:", forward)
# 7.输出坐标
return left, right, backward, forward
# 车身姿态修正:
# 1.本次车辆的倾斜角度与上次的倾斜角度在误差范围外:只做轮子的转动
# (forward方向) 向左歪则轮子向右(逆时针)转动;向右歪则轮子向左(顺时针)转动。
# (backward方向)向左歪则轮子向左(顺时针)转动;向右歪则轮子向右(逆时针)转动。
# 2.本次车辆的倾斜角度与上次的倾斜角度在误差范围内:只做车身的调整
# (forward方向) 向左歪则车身向右微调;向右歪则车身向左微调。
# (backward方向)向左歪则车身向左微调;向右歪则车身向右微调。
# 车身转弯:
# 无论是forward还是backward方向,车身左转和车身右转不变。
# 转弯
'''
direction1:前一个方向(0-359)
direction2:后一个方向(0-359)
车身转弯
'''
def Turn_Angle(direction1, direction2):
angle = direction1 - direction2
if 5 > angle > -5 or 355 <= angle <= 360 or -360 <= angle <= -355: # 默认误差,直行
message = "stop"
print("不转")
elif 5 <= angle < 180: # 右转(轮子顺时针转,后退)
message = "backward"
print("右转")
elif 180 <= angle < 355: # 左转(轮子顺时针转,前进)
message = "forward"
print("左转")
elif -5 >= angle > -180: # 左转
message = "forward"
print("左转")
elif -180 >= angle > -355: # 右转
message = "backward"
print("右转")
return message
# 循环中所需参数
'''
车前后中点的位置:
车身长度为170,
红色位置世界坐标A[x1,y1]在车身左后方,
绿色位置世界坐标B[x2,y2]在车身右后方,
车身的后方中点坐标为s = [(x1+x2)/2,(y1+y2)/2],
AB两点线段半长为:u = {[(y2-y1)**2+(x2-x1)**2]**0.5}/2
AB中心点与B点水平距离x' = |(x2-x1)|/2
AB中心点与B点垂直距离y' = |(y2-y1)|/2
车身前端距车身后端水平距离$x = (|(y2-y1)|*L)/2u
车身前端距车身后端垂直距离$y = (|(x2-x1)|*L)/2u
车身的前方中点坐标为:
(1)x1 = x2 : y1 > y2 坐标为:[xs+$x,ys+$y]
y1 < y2 坐标为:[xs-$x,ys-$y]
(2)x1 > x2 : (y2-y1)/(x2-x1) > 0 坐标为:[xs+$x,ys-$y]
(y2-y1)/(x2-x1) <= 0 坐标为:[xs-$x,ys-$y]
(3)x1 < x2 : (y2-y1)/(x2-x1) > 0 坐标为:[xs-$x,ys+$y]
(y2-y1)/(x2-x1) <= 0 坐标为:[xs+$x,ys+$y]
S : 后方中点
V : 前方中点
'''
def MidPoint(A, B):
V = []
L = 70
S = [(A[0] + B[0]) / 2, (A[1] + B[1]) / 2]
u = (((B[1] - A[1]) ** 2 + (B[0] - A[0]) ** 2) ** 0.5) // 2
n_x = abs(B[0] - A[0]) // 2
n_y = abs(B[1] - A[1]) // 2
t_x = (n_y * L) // u
t_y = (n_x * L) // u
if A[0] == B[0]:
if A[1] > B[1]:
V = [(S[0] + t_x), (S[1] + t_y)]
elif A[1] < B[1]:
V = [(S[0] - t_x), (S[1] - t_y)]
elif A[0] > B[0]:
if ((B[1] - A[1]) / (B[0] - A[0])) > 0:
V = [(S[0] + t_x), (S[1] - t_y)]
elif ((B[1] - A[1]) / (B[0] - A[0])) <= 0:
V = [(S[0] - t_x), (S[1] - t_y)]
elif A[0] < B[0]:
if ((B[1] - A[1]) / (B[0] - A[0])) > 0:
V = [(S[0] - t_x), (S[1] + t_y)]
elif ((B[1] - A[1]) / (B[0] - A[0])) <= 0:
V = [(S[0] + t_x), (S[1] + t_y)]
return S, V
def MidPoint2(A, B):
L = 85
S = [(A[0] + B[0]) / 2, (A[1] + B[1]) / 2]
dx = B[0] - A[0]
dy = B[1] - A[1]
length = (dx ** 2 + dy ** 2) ** 0.5
if length == 0:
return (S, S) # A和B重合时返回同一点
# 计算垂直于AB方向的单位向量(逆时针方向)
perp_dx = -dy / length
perp_dy = dx / length
# 计算位移量
delta_x = perp_dx * L
delta_y = perp_dy * L
# 确定V点坐标
V = [round((S[0] + delta_x), 1), round((S[1] + delta_y), 1)]
return S, V
'''
车身的角度:
车身前后中点的斜率,使用与x轴正方向的夹角角度作为数据,0-359
S,V = 后方中点,前方中点
道路的角度:
S,V = 起点坐标,终点坐标
'''
def increasing(lst):
for i in range(len(lst) - 1):
if lst[i] >= lst[i + 1]:
return False
return True
def decreasing(lst):
for i in range(len(lst) - 1):
if lst[i] <= lst[i + 1]:
return False
return True
def Angle(S, V):
angle = 0
if S[0] == V[0]:
if S[1] > V[1]:
angle = 270
elif S[1] < V[1]:
angle = 90
else:
k = (V[1] - S[1]) / (V[0] - S[0])
if k >= 0:
if S[0] > V[0]:
angle = int((180 * math.atan(k)) / math.pi) + 180
elif S[0] < V[0]:
angle = int((180 * math.atan(k)) / math.pi)
elif k < 0:
if S[0] > V[0]:
angle = int((180 * math.atan(k)) / math.pi) + 180
elif S[0] < V[0]:
angle = int((180 * math.atan(k)) / math.pi) + 360
print("angle:", angle)
return angle
if __name__ == '__main__':
while True:
data = ditu()
while True:
list = eval(data)
# 如果接收到的信息为"exit",退出循环
if data == "exit":
break
# list = newlb # [[起点世界坐标,中间点世界坐标,ip1],[中间点世界坐标,终点世界坐标,ip2]]
print(list)
a = 0
# 2.读取路径并遍历执行
for i in list:
print("i:", i)
left, right, backpoint, forpoint = Coord_print(i[2]) # 0.36s-0.43s
a += 1
print("车当前角度:")
angle_car = Angle(backpoint, forpoint)
print("路径角度:")
angle_road = Angle(i[0], i[1])
# 3.判断转弯
message1 = Turn_Angle(angle_car, angle_road)
'''
forward--左转--unclock
backward--右转--clock
'''
if message1 == "stop": # 不转弯,直行
# Input("forward")
pass
else:
Deviation_angle1 = abs(angle_car - angle_road)
if Deviation_angle1 > 180:
Deviation_angle1 = 360 - Deviation_angle1
# elif Deviation_angle < -180:
# Deviation_angle = Deviation_angle + 360
angle_i = str(Deviation_angle1)
if message1 == "backward": # 车身右转
# Input("clock90")
Input("yuo" + angle_i)
elif message1 == "forward": # 车身左转
# Input("unclock90")
Input("zuo" + angle_i)
message2 = "stop"
# 判断转弯是否完成
while True:
left, right, backpoint, forpoint = Coord_print(i[2]) # 0.36s-0.43s
a += 1
print("车当前角度:")
angle_car = Angle(backpoint, forpoint)
message = Turn_Angle(angle_car, angle_road)
# 偏差角度
Deviation_angle = abs(angle_car - angle_road)
if Deviation_angle > 180:
Deviation_angle = 360 - Deviation_angle
# elif Deviation_angle < -180:
# Deviation_angle = Deviation_angle + 360
angle_i = str(Deviation_angle)
if message == "backward": # 车身右转
# Input("clock90")
Input("yuo" + angle_i)
elif message == "forward": # 车身左转
# Input("unclock90")
Input("zuo" + angle_i)
if (message == "stop") or ((message2 != "stop") and (message2 != message)):
# 车轮回正
# Input("return")
# time.sleep(0.1)
# Input("forward")
# break
if message1 == "backward":
Input("shun030090")
time.sleep(0.1)
break
elif message1 == "forward":
Input("nnii030090")
time.sleep(0.1)
break
elif message2 == "backward":
Input("shun030090")
time.sleep(0.1)
break
elif message2 == "forward":
Input("nnii030090")
time.sleep(0.1)
break
message2 = message
# 4.直行
# Input("return")
# time.sleep(0.1)
Input("forward")
time.sleep(0.3)
Input("v2500")
# time.sleep(0.1)
# Input("v2500")
# time.sleep(0.1)
# Input("return")
while True:
# 5.判断是否到达目标点,
left, right, backpoint, forpoint = Coord_print(i[2])
a += 1
print("车当前角度:")
angle_car = Angle(backpoint, forpoint)
# angle_road = Angle(i[0], i[1])
# Input("forward")
if (angle_road == 0) and (forpoint[0] >= i[1][0] - 35):
time.sleep(0.1)
Input("stop")
break
elif (angle_road == 90) and (forpoint[1] >= i[1][1] - 35):
time.sleep(0.1)
Input("stop")
break
elif (angle_road == 180) and (forpoint[0] <= i[1][0] - 35):
time.sleep(0.1)
Input("stop")
break
elif (angle_road == 270) and (forpoint[1] <= i[1][1] - 35):
time.sleep(0.1)
Input("stop")
break
# 直线纠偏
Deviation_angle2 = angle_car - angle_road
if Deviation_angle2 > 180:
Deviation_angle2 = 360 - Deviation_angle2
if Deviation_angle2 >= 4: # 向右纠偏
angle_d = str(Deviation_angle2)
Input("stop")
time.sleep(0.1)
Input("nnii030010")
time.sleep(0.1)
Input("right" + angle_d)
time.sleep(0.1)
Input("shun030010")
time.sleep(0.1)
Input("forward")
elif Deviation_angle2 <= -4: # 向左纠偏
angle_d = str(Deviation_angle2)
Input("stop")
time.sleep(0.1)
Input("shun030010")
time.sleep(0.1)
Input("left" + angle_d)
time.sleep(0.1)
Input("nnii030010")
time.sleep(0.1)
Input("forward")
else:
pass
break
# if abs(Deviation_angle) > 5:
# Input("return")
# if Deviation_angle > 5: # 左歪 >> 右转(顺时针) >> 轮逆时针转
# # 1、车停轮子微调再继续前进将车身带正
# Input("stall")
# Input("nnii030020")
# Input("forward")
# Input("yuo" + angle_i)
# # Input("shun030020")
# Input("forward")
# elif Deviation_angle < -5: # 右歪 >> 左转(逆时针)
# Input("stall")
# Input("shun030020")
# Input("forward")
# Input("zuo" + angle_i)
# # Input("nnii030020")
# Input("forward")
# 6.纠偏
# if a % 5 == 0:
# car_angle_list = []
# car_angle_list.append(angle_car)
# elif a % 5 == 1:
# car_angle_list.append(angle_car)
# elif a % 5 == 2:
# car_angle_list.append(angle_car)
# elif a % 5 == 3:
# car_angle_list.append(angle_car)
# elif a % 5 == 4:
# car_angle_list.append(angle_car)
# # 分析车角度变化情况,判断车轮是否回正
# if increasing(car_angle_list):
# pass
# elif decreasing(car_angle_list):
# pass
# else:
# # a:车轮已回正时,车身的角度与路径方向有偏差
# if Deviation_angle > 5: # 左歪 >> 右转(顺时针) >> 轮逆时针转
# # 1、车停轮子微调再继续前进将车身带正
# Input("stall")
# Input("nnii030020")
# Input("forward")
# Input("yuo" + angle_i)
# # Input("shun030020")
# Input("forward")
# elif Deviation_angle < -5: # 右歪 >> 左转(逆时针)
# Input("stall")
# Input("shun030020")
# Input("forward")
# Input("zuo" + angle_i)
# # Input("nnii030020")
# Input("forward")
# else:
# pass
代码解释
最新发布