def getmeanpoint(self, data):
# TODO e
# TODO e END
wrap_img = perspective_transform(new_image, M, (580, 560))
yellow_line = apply_yellow_white_mask(wrap_img)
# TODO I
# TODO I END
self.planning_path = Trajectory()
#print("point size:",str(len(mean_y)))
if len(mean_y) > 0:
mean_x_real, mean_y_real = translation_view(
np.asarray(mean_x), np.asarray(mean_y))
for i, point in enumerate(mean_y_real):
point_xy = Point()
point_xy.x = mean_x_real[i]
point_xy.y = point
self.planning_path.point.append(point_xy)
def getmeanpoint(self, data):
# TODO e
new_image = np.frombuffer(data.data, dtype=np.uint8)
new_image = cv2.imdecode(new_image, cv2.IMREAD_COLOR)
wrap_img = perspective_transform(new_image, M, (580, 560))
yellow_line = apply_yellow_white_mask(wrap_img)
# TODO I
line_list, mean_x, mean_y = find_line_fit(yellow_line,
midpoint=car_mid_point,
nwindows=10,
margin=100)
self.planning_path = Trajectory()
#print("point size:",str(len(mean_y)))
if len(mean_y) > 0:
mean_x_real, mean_y_real = translation_view(
np.asarray(mean_x), np.asarray(mean_y))
for i, point in enumerate(mean_y_real):
point_xy = Point()
point_xy.x = mean_x_real[i]
point_xy.y = point
self.planning_path.point.append(point_xy)
def getmeanpoint(self, data):
#print(data)
new_image = np.frombuffer(data.data, dtype=np.uint8)
new_image = cv2.imdecode(new_image, cv2.IMREAD_COLOR)
img = cv2.resize(new_image, (424, 408))
wrap_img = perspective_transform(img, M, img_size=(444, 343))
wrap_img3 = get_tag_mask(wrap_img)
binary = abs_sobel_thresh(wrap_img3, orient='y', sobel_kernel=3, thresh=(20, 255))
mean_x, mean_y, out_img2 = find_line_fit(binary, midpoint=car_mid_point, margin=100)
self.planning_path = Trajectory()
if len(mean_y) > 0:
mean_x_real, mean_y_real = translation_view(np.asarray(mean_x), np.asarray(mean_y))
print(mean_x_real, mean_y_real)
for i, point in enumerate(mean_y_real):
point_xy = Point()
point_xy.x = mean_x_real[i]
point_xy.y = point
self.planning_path.point.append(point_xy)
def reshape(self, data):
point_array = data.point
self.planning_path = Trajectory()
for i, point in enumerate(point_array):
point_xy = Point()
new_x, new_y = translation_view(point.x, point.y)
point_xy.x = new_x
point_xy.y = new_y
self.planning_path.point.append(point_xy)
def plan(self):
print(hasattr(self, 'data'))
while not cyber.is_shutdown():
print("in plan ********************************************")
if not hasattr(self, 'data'):
time.sleep(0.2)
print("no data sleep firstly")
continue
sx = self.data.start_point.x # start x position [m]
sy = self.data.start_point.y # start y positon [m]
gx = self.data.end_point.x # goal x position [m]
gy = self.data.end_point.y # goal y position [m]
grid_size = 0.051 # potential grid size [m]
robot_radius = 0.0725 # robot radius [m]
print('start point,{},{} goal point,{}, {}'.format(sx, sy, gx, gy))
ox, oy = [], []
for obstacle in self.data.obs_points:
ox.append(obstacle.x)
oy.append(obstacle.y)
print('obstacle information, ox:{}, oy:{} '.format(ox, oy))
# ox = [-0.03464780002832413] # obstacle x position list [m]
# oy = [0.6250196099281311] # obstacle y position list [m]
# if show_animation:
# plt.grid(True)
# plt.axis("equal")
# path generation
rx, ry = [], []
start = time.time()
rx, ry = potential_field_planning(
sx, sy, gx, gy, ox, oy, grid_size, robot_radius)
end = time.time()
print('time cost: {}'.format(end - start))
print('rx,{}, ry.{}'.format(rx, ry))
self.planning_path = Trajectory()
if not rx:
print("Failed to find a path")
else:
point = Point()
for i, _ in enumerate(rx):
point.x = rx[i]
point.y = ry[i]
self.planning_path.point.append(point)
print('trajectory,{}'.format(self.planning_path))
self.write_to_channel()
def searchNear(self, minF, offsetX, offsetY):
"""
搜索节点周围的点, 更新openlist, 重新计算G值、设置father(如有需要)
:param minF:
:param offsetX:
:param offsetY:
:return:
"""
# 越界检测
if minF.point.x + offsetX < map_x_min or minF.point.x + offsetX > map_x_max - 1 or minF.point.y + offsetY < map_y_min or minF.point.y + offsetY > map_y_max - 1:
return
# 如果是障碍，就忽略
for obs in obslist:
if obs[0] == minF.point.x + offsetX and obs[
1] == minF.point.y + offsetY:
return
# 如果在关闭表中，就忽略
if self.pointInCloseList(
Point(minF.point.x + offsetX, minF.point.y + offsetY)):
return
# 设置单位花费
if offsetX == 0 or offsetY == 0:
step = 7
else:
step = 14
if offsetX > 0 and offsetY >= 0:
step = step + 14
# 如果不在openList中，就把它加入openlist
currentNode = self.pointInOpenList(
Point(minF.point.x + offsetX, minF.point.y + offsetY))
if not currentNode:
currentNode = AStar.Node(Point(minF.point.x + offsetX,
minF.point.y + offsetY),
self.endPoint,
g=minF.g + step)
currentNode.father = minF
self.openList.append(currentNode)
return
# 如果在openList中，判断minF到当前点的G是否更小
if minF.g + step < currentNode.g: # 如果更小，就重新计算g值，并且改变father
currentNode.g = minF.g + step
currentNode.father = minF
def callback(self, data):
global obslist, map_w, map_h, map_x_min, map_x_max, map_y_min, map_y_max
obslist = []
pStart_x = int(20 * data.start_point.x)
pStart_y = int(20 * data.start_point.y)
pEnd_x = int(20 * data.end_point.x)
pEnd_y = int(20 * data.end_point.y - 1)
pStart = Point(pStart_x, pStart_y)
pEnd = Point(pEnd_x, pEnd_y)
for point in data.obs_points:
obslist.append([int(20 * point.x), int(20 * point.y)])
for i in range(0):
obslist.append(
[random.uniform(2, pEnd.y),
random.uniform(2, pEnd.y)])
#time_start = time.time()
# 创建AStar对象,并设置起点终点
aStar = AStar(pStart, pEnd)
aStar.expansion(offset=9)
# 开始寻路
pathList = aStar.start()
#time_end = time.time()
#print('totally cost', time_end - time_start)
self.planning_path = Trajectory()
if not pathList:
print("Failed to find a path")
else:
for path_point in pathList:
point_xy.x = path_point.x * 0.05
point_xy.y = path_point.y * 0.05
self.planning_path.point.append(point_xy)
if not cyber.is_shutdown() and self.planning_path:
self.writer.write(self.planning_path)
# coding:utf-8
import os
import sys
import time
import signal
from cyber_py import cyber
from modules.planning.proto.planning_pb2 import Trajectory
from modules.planning.proto.planning_pb2 import Point
from modules.localization.proto.localization_pb2 import localization
from modules.localization.proto.localization_pb2 import pos
import cv2
import numpy as np
point_xy = Point()
maps = cv2.imread("maps1.jpeg", cv2.IMREAD_GRAYSCALE) # 读取地图图像，灰度读入。灰度为0表示障碍物
maps_size = np.array(maps) # 获取图像行和列大小
hight = maps_size.shape[0] # 行数->y
width = maps_size.shape[1] # 列数->x
scale = 144.9
class planning(object):
def __init__(self, node):
self.node = node
self.start_x = 0
self.start_y = 0
self.goal_x = 0
def getmeanpoint(self, data):
new_image = np.frombuffer(data.data, dtype=np.uint8)
new_image = cv2.imdecode(new_image, cv2.IMREAD_COLOR)
img = cv2.resize(new_image, (424, 408))
wrap_img = perspective_transform(img, M, img_size=(444, 343))
gray_d = cv2.cvtColor(wrap_img, cv2.COLOR_BGR2GRAY)
wrap_img_2 = cv2.threshold(gray_d, 100, 220, cv2.THRESH_BINARY_INV)[1]
# TODO e begin
kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3))
img_d = cv2.erode(wrap_img_2, kernel, iterations=2)
wrap_img_2 = cv2.dilate(img_d, kernel, iterations=3)
# TODO e end
cv2.fillPoly(img_d, mask_right_cor, 0)
# TODO e begin
image, contours, hierarchy = cv2.findContours(
wrap_img_2, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_TC89_KCOS)
# TODO e end
# TODO f begin
wrap_img3 = findMaxContour(wrap_img, wrap_img_2, contours)
# TODO f end
# TODO g begin
binary = abs_sobel_thresh(wrap_img3,
orient='x',
sobel_kernel=3,
thresh=(20, 255))
left_list, right_list, mean_x, mean_y, out_img2 = find_line_fit(
binary, midpoint=car_mid_point, margin=100)
# TODO g end
self.planning_path = Trajectory()
self.planning_path_left = Trajectory()
self.planning_path_right = Trajectory()
if len(mean_y) > 0:
mean_x_real, mean_y_real = translation_view(
np.asarray(mean_x), np.asarray(mean_y))
left_list_real, left_y_real = translation_view(
np.asarray(left_list), np.asarray(mean_y))
right_list_real, right_y_real = translation_view(
np.asarray(right_list), np.asarray(mean_y))
for i, point in enumerate(mean_y_real):
point_xy = Point()
point_xy.x = mean_x_real[i]
point_xy.y = point
self.planning_path.point.append(point_xy)
point_xy = Point()
point_xy.x = left_list_real[i]
point_xy.y = left_y_real[i]
self.planning_path_left.point.append(point_xy)
point_xy = Point()
point_xy.x = right_list_real[i]
point_xy.y = right_y_real[i]
self.planning_path_right.point.append(point_xy)
print("left:",
np.asarray(left_list_real).mean(), "right:",
np.asarray(right_list_real).mean(), "mean:",
np.asarray(mean_x_real).mean())