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)
