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 __init__(self, node):
self.node = node
self.speed = 0
self.target_speed = 0
self.lateral_error = 0
self.cmd = Control_Command()
self.cmd.steer_angle = 0
self.cmd.throttle = 0
self.trajectory = Trajectory()
self.sum_error_longi = 0
self.node.create_reader("/chassis", Chassis, self.chassiscallback)
self.node.create_reader("/control_reference", Control_Reference,
self.speedrefcallback)
# /planning/trajectory\
# /planning/control_trajectory
self.node.create_reader("/perception/road_mean_point", Trajectory,
self.trajectorycallback)
self.writer = self.node.create_writer("/control", Control_Command)
signal.signal(signal.SIGINT, self.sigint_handler)
signal.signal(signal.SIGHUP, self.sigint_handler)
signal.signal(signal.SIGTERM, self.sigint_handler)
self.is_sigint_up = False
while True:
time.sleep(0.05)
self.lateral_controller(self.trajectory, self.lateral_error)
self.longitude_controller(self.target_speed, self.speed)
self.writer.write(self.cmd)
if self.is_sigint_up:
print("Exit")
self.is_sigint_up = False
return
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 __init__(self, node):
self.node = node
self.speed = 0
self.target_speed = 0
self.lateral_error = 0
self.cmd = Control_Command()
self.trajectory = Trajectory()
self.node.create_reader("/chassis", Chassis, self.chassiscallback)
self.node.create_reader("/control_reference", Control_Reference,
self.speedrefcallback)
self.node.create_reader("/planning/dwa_trajectory", Trajectory,
self.trajectorycallback)
self.writer = self.node.create_writer("/control", Control_Command)
signal.signal(signal.SIGINT, self.sigint_handler)
signal.signal(signal.SIGHUP, self.sigint_handler)
signal.signal(signal.SIGTERM, self.sigint_handler)
self.is_sigint_up = False
while True:
try:
time.sleep(0.05)
self.lateral_controller(self.trajectory, self.lateral_error)
self.longitude_controller(self.target_speed, self.speed)
self.writer.write(self.cmd)
if self.is_sigint_up:
print("Exit")
self.is_sigint_up = False
return
except Exception:
break
def __init__(self, node):
self.node = node
self.start_x = 0
self.start_y = 0
self.goal_x = 0
self.goal_y = 0
self.global_path = Trajectory()
self.node.create_reader("/geek/uwb/localization", pos,
self.localizationcallback)
self.node.create_reader("/planning/mission_point", Point,
self.missioncallback)
self.writer = self.node.create_writer("/planning/global_trajectory",
Trajectory)
signal.signal(signal.SIGINT, self.sigint_handler)
signal.signal(signal.SIGHUP, self.sigint_handler)
signal.signal(signal.SIGTERM, self.sigint_handler)
self.is_sigint_up = False
while True:
time.sleep(0.05)
if not cyber.is_shutdown() and self.global_path:
self.writer.write(self.global_path)
if self.is_sigint_up:
print("Exit!")
self.is_sigint_up = False
sys.exit()
def __init__(self, node):
self.node = node
self.planning_path = Trajectory()
self.planning_path_left = Trajectory()
self.planning_path_right = Trajectory()
# TODO create reader
self.node.create_reader("/realsense/compressed_image", Image, self.callback)
# TODO create writer
self.writer = self.node.create_writer(
"/perception/road_mean_point", Trajectory)
self.writer_left = self.node.create_writer(
"/perception/road_left_point", Trajectory)
self.writer_right = self.node.create_writer(
"/perception/road_right_point", Trajectory)
def __init__(self, node):
self.node = node
self.planning_path = Trajectory()
# TODO create reader
self.node.create_reader("/perception/get_point", Trajectory,
self.callback)
# TODO create writer
self.writer = self.node.create_writer("/perception/translation_point",
Trajectory)
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 missioncallback(self, Point):
self.goal_x = int(Point.x)
self.goal_y = int(Point.y)
pathList = self.start(self.start_x, self.start_y, self.goal_x,
self.goal_y)
self.planning_path = Trajectory()
if not pathList:
print("Failed to find a path")
else:
for path_point in pathList:
point_xy.x = path_point[0]
point_xy.y = path_point[1]
self.planning_path.point.append(point_xy)
if not cyber.is_shutdown() and self.planning_path:
self.writer.write(self.planning_path)
def callback(self, data):
global obslist, planning_path, best_trajectory
obslist = []
print(" start!!")
# initial state [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
x = np.array([0.0, 0.0, math.pi / 2.0, 0.3, 0.0])
# goal position [x(m), y(m)]
goal = np.array([data.end_point.x, data.end_point.y])
# obstacles [x(m) y(m), ....]
for point in data.obs_points:
obslist.append([point.x, point.y])
ob = np.matrix(obslist)
# input [forward speed, yawrate]
u = np.array([0.3, 0.0])
config = Config()
best_trajectory = np.array(x)
u, best_trajectory = dwa_control(x, u, config, goal, ob)
x = motion(x, u, config.dt)
self.planning_path = Trajectory()
if not best_trajectory.any():
print("Failed to find a path")
else:
for path_point in best_trajectory:
point_xy.x = path_point[0]
point_xy.y = path_point[1]
self.planning_path.point.append(point_xy)
if not cyber.is_shutdown() and self.planning_path:
self.writer.write(self.planning_path)
print("Done")
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)
def callback(self, pos):
global obslist, planning_path, best_trajectory
start_x = int(pos.x * scale)
start_y = int(pos.y * scale)
obslist = []
num_point = 0
minr = float("inf")
for i, point in enumerate(self.pathList):
r = math.sqrt((point[0] - start_x)**2 + (point[1] - start_y)**2)
if minr >= r:
minr = r
num_point = i
if (num_point - 8) <= 0:
num_point = 8
#规划预瞄点
f_point = self.pathList[num_point - 8]
self.goal = f_point
print("goal:", self.goal)
#将预瞄点坐标从地图坐标系转换到车身坐标系
f_point = [(f_point[0] - start_x) / scale,
-((f_point[1] - start_y) / scale)]
yaw = math.pi - pos.yaw
x_f = f_point[0] * math.cos(yaw) + f_point[1] * math.sin(yaw)
y_f = f_point[1] * math.cos(yaw) - f_point[0] * math.sin(yaw)
print("trans-goal:", [x_f, y_f])
#将预设障碍物坐标从地图坐标系转换到车身坐标系
ob_point = [643, 353]
ob_point = [(ob_point[0] - start_x) / scale,
-((ob_point[1] - start_y) / scale)]
x_ob = ob_point[0] * math.cos(yaw) + ob_point[1] * math.sin(yaw)
y_ob = ob_point[1] * math.cos(yaw) - ob_point[0] * math.sin(yaw)
print("ob:", [x_ob, y_ob])
print(" planning start!")
# initial state [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
x = np.array([0.0, 0.0, 0, 0.3, 0.0])
# goal position [x(m), y(m)]
goal = np.array([x_f, y_f])
ob = np.matrix(self.obstacleList)
ob = np.array([[x_ob, y_ob, 0.24 / scale]])
# input [forward speed, yawrate]
u = np.array([0.3, 0.0])
config = Config()
best_trajectory = np.array(x)
u, best_trajectory = dwa_control(x, u, config, goal, ob)
x = motion(x, u, config.dt)
#接近终点减速
dist_car_goal = ((((start_x - self.pathList[0][0])**2 +
(start_y - self.pathList[0][1])**2)**0.5) / scale)
if dist_car_goal <= 0.2:
u[0] = 0
self.planning_path = Trajectory()
self.speed = Control_Reference()
self.speed.vehicle_speed = u[0]
if not best_trajectory.any():
print("Failed to find a path")
else:
for path_point in best_trajectory:
point_xy.x = path_point[0]
point_xy.y = path_point[1]
self.planning_path.point.append(point_xy)
point_xy.x = self.goal[0]
point_xy.y = self.goal[1]
self.planning_path.point.append(point_xy)
if not cyber.is_shutdown() and self.planning_path:
self.writer.write(self.planning_path)
self.vwriter.write(self.speed)
print("planning done")
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())
