#way_points = astar.planning(pos_int((pos[0],pos[1])), pos_int(nav_pos_new),inter=20)
if len(way_points) > 1:
nav_pos = nav_pos_new
path = np.array(cubic_spline_2d(way_points, interval=4))
controller.set_path(path)
def pos_int(p):
return (int(p[0]), int(p[1]))
cv2.namedWindow('test')
cv2.setMouseCallback('test', mouse_click)
while (True):
car.update()
print("\rState: " + car.state_str(), "| Goal:", nav_pos, end="\t")
pos = (car.x, car.y, car.yaw)
sdata = lmodel.measure(pos)
plist = EndPoint(pos, [61, -120, 120], sdata)
# Map
gm.update_map(pos, [61, -120, 120, 250], sdata)
mimg = gm.adaptive_get_map_prob()
mimg = (255 * mimg).astype(np.uint8)
mimg = cv2.cvtColor(mimg, cv2.COLOR_GRAY2RGB)
mimg_ = cv2.flip(mimg, 0)
cv2.imshow("map", mimg_)
#
img_ = img.copy()
for pts in plist:
for i in range(path.shape[0] - 1):
cv2.line(img_path, (int(path[i, 0]), int(path[i, 1])),
(int(path[i + 1, 0]), int(path[i + 1, 1])), (1.0, 0.5, 0.5),
1)
print("\rimg_path 2=")
print(img_path)
# Initialize Car
car = KinematicModel()
start = (50, 300, 0)
car.init_state(start)
controller = PurePursuitControl(kp=1, Lfc=10)
controller.set_path(path)
while (True):
print("\rState: " + car.state_str(), end="\t")
# ================= Control Algorithm =================
# PID Longitude Control
#print("\rpath=")
#print(path)
end_dist = np.hypot(path[-1, 0] - car.x, path[-1, 1] - car.y)
target_v = 40 if end_dist > 265 else 0
next_a = 0.1 * (target_v - car.v)
# Pure Pursuit Lateral Control
state = {
"x": car.x,
"y": car.y,
"yaw": car.yaw,
"v": car.v,
"l": car.l