#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