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))
left_list, right_list, mean_x, mean_y, out_img2 = find_line_fit(binary, midpoint=car_mid_point, margin=100)
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))
# print(mean_x_real, mean_y_real)
#print(right_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)
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())
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):
# 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 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 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())