car = KinematicModel(l=20, d=5, wu=5, wv=2, car_w=14, car_f=25, car_r=5) control_type = 2 if control_type == 0: controller = PidControl() elif control_type == 1: controller = PurePursuitControl(kp=0.7, Lfc=10) elif control_type == 2: controller = StanleyControl(kp=0.5) elif control_type == 3: controller = LQRControl() else: controller = PurePursuitControl(kp=0.7, Lfc=10) pos = (100, 200, 0) car.x = 100 car.y = 200 car.yaw = 0 rrt = RRTStar(m_dilate) astar = AStar(m_dilate) gm = GridMap([0.5, -0.5, 5.0, -5.0], gsize=3) def mouse_click(event, x, y, flags, param): global nav_pos, pos, path, m_dilate, way_points, controller if event == cv2.EVENT_LBUTTONUP: nav_pos_new = (x, m.shape[0] - y) if m_dilate[nav_pos_new[1], nav_pos_new[0]] > 0.5: way_points = rrt.planning((pos[0], pos[1]), nav_pos_new, 20) #way_points = astar.planning(pos_int((pos[0],pos[1])), pos_int(nav_pos_new),inter=20) if len(way_points) > 1:
# Read Image img = cv2.flip(cv2.imread("../Maps/map.png"), 0) img[img > 128] = 255 img[img <= 128] = 0 m = np.asarray(img) m = cv2.cvtColor(m, cv2.COLOR_RGB2GRAY) m = m.astype(float) / 255. img = img.astype(float) / 255. # Lidar lmodel = LidarModel(m) car = KinematicModel() pos = (100, 200, 0) car.x = 100 car.y = 200 car.yaw = 0 gm = GridMap([0.5, -0.5, 5.0, -5.0], gsize=3) while (True): print("\rState: " + car.state_str(), end="\t") car.update() 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)