def main():
exit_keys = (pygame.K_ESCAPE, )
background_colour = (255, 255, 255)
width, height = (1400, 750)
screen = pygame.display.set_mode((width, height))
pygame.font.init()
font = pygame.font.SysFont('Helvetica', 16)
pygame.display.set_caption('CarSim')
screen.fill(background_colour)
pygame.display.flip()
running = True
waypoints = []
car = Car(width/2, height/2)
dt = 0
last = time.time_ns()
pt = PositionTracker()
pp = PathPlanner(car, pt)
while running:
for event in pygame.event.get():
if event.type == pygame.QUIT or (event.type == pygame.KEYDOWN and event.key in exit_keys):
running = False
break
if event.type == pygame.MOUSEBUTTONDOWN:
x, y = pygame.mouse.get_pos()
waypoints.append(Waypoint(x, y))
if event.type == pygame.KEYDOWN and event.key == pygame.K_r:
if car.cruise_control_on:
car.disable_cruise_control()
else:
car.enable_cruise_control(80)
if event.type == pygame.KEYDOWN and event.key == pygame.K_q:
if car.front_wheel.has_target and car.front_wheel.target_rotation < 0:
car.unset_steer_target()
else:
car.steer_target(20, Direction.LEFT)
if event.type == pygame.KEYDOWN and event.key == pygame.K_e:
if car.front_wheel.has_target and car.front_wheel.target_rotation > 0:
car.unset_steer_target()
else:
car.steer_target(20, Direction.RIGHT)
pressed = pygame.key.get_pressed()
if pressed[pygame.K_w]:
car.accelerate()
if pressed[pygame.K_a]:
car.steer_left(dt)
if pressed[pygame.K_d]:
car.steer_right(dt)
if pressed[pygame.K_s]:
car.apply_brake(dt)
if pressed[pygame.K_SPACE]:
car.full_speed()
car.update(dt)
if car.velocity:
pt.add(car.position)
pp.plan(waypoints)
screen.fill(background_colour)
for wp in waypoints:
pygame.draw.circle(screen, Waypoint.sec_color, (wp.x, wp.y), wp.tolerance)
pygame.draw.circle(screen, Waypoint.color, (wp.x, wp.y), wp.radius)
# pygame.draw.rect(screen, (255, 0, 0), car.rect)
screen.blit(car.image, car.rect)
# pygame.draw.line(screen, car.sensor.color, car.sensor.center, car.sensor.get_end(), 2)
speed = font.render(f'v={round(car.velocity)} kmh', True, (0, 0, 0))
screen.blit(speed, (width-speed.get_width()-5, 5))
cc = font.render(f'CC {("off", "on")[car.cruise_control_on]}', True, (0, 0, 0))
screen.blit(cc, (width-cc.get_width()-5, 10 + speed.get_height()))
wheel = font.render(f'Wheel: {round(car.front_wheel.rotation)}°', True, (0, 0, 0))
screen.blit(wheel, (width - wheel.get_width() - 5, 15 + speed.get_height()*2))
target = font.render(f'Target: {round(car.front_wheel.target_rotation)}°', True, (0, 0, 0))
screen.blit(target, (width - target.get_width() - 5, 20 + speed.get_height() * 3))
rot = font.render(f'Car rotation: {None if pt.rotation is None else round(pt.rotation * 100) / 100}°', True, (0, 0, 0))
screen.blit(rot, (width - rot.get_width() - 5, 25 + speed.get_height() * 4))
des = font.render(f'Desired rotation: {None if pp.desired_heading is None else round(pp.desired_heading * 100) / 100}°', True, (0, 0, 0))
screen.blit(des, (width - des.get_width() - 5, 30 + speed.get_height() * 5))
pygame.display.flip()
current = time.time_ns()
dt = (current - last) / 10 ** 9
last = current
class CarController:
def __init__(self, map_name='campus'):
self.map = MapLoader.load(path='resources/campus.json')
self.pt = PositionTracker(osmap=self.map)
self.pf = PositionFetcher(self.pt)
self.planner = PathPlanner(self.pt, on_wp_reached=self.remove_waypoint)
self.vis = Visualizer(map_name=map_name,
pt=self.pt,
add_wp=self.add_waypoint,
osmap=self.map)
self.waypoints_lock = Lock()
self.waypoints_changed = True
self.waypoints: Optional[List[Waypoint]] = None
self.set_waypoints([])
def get_waypoints(self) -> List[Waypoint]:
with self.waypoints_lock:
return self.waypoints[:]
def on_waypoint_change(self) -> None:
self.waypoints_changed = True
self.vis.set_waypoints(self.waypoints[:])
def remove_waypoint(self, waypoint_id: int) -> None:
with self.waypoints_lock:
self.waypoints = list(
filter(lambda wp: wp.id != waypoint_id, self.waypoints))
self.on_waypoint_change()
def update_position(self) -> None:
if not self.pf.fetch():
self.planner.predict_position()
def add_waypoint(self, waypoint: Waypoint) -> None:
with self.waypoints_lock:
orig_pos = Position(lat=waypoint.lat, lon=waypoint.lon)
adjusted_position, path = self.map.closest_point(orig_pos)
waypoint.path = path
waypoint.lat = adjusted_position.lat
waypoint.lon = adjusted_position.lon
self.waypoints.append(waypoint)
self.on_waypoint_change()
def set_waypoints(self, waypoints: List[Waypoint]) -> None:
with self.waypoints_lock:
self.waypoints = waypoints
self.on_waypoint_change()
def init_position(self):
for i in range(1, 0, -1):
self.update_position()
self.vis.update()
print(i)
time.sleep(1.0)
def follow_waypoints(self) -> None:
self.set_waypoints([
# Waypoint(18.1622607, 49.8358360),
# Waypoint(18.1620518, 49.8356722)
# Waypoint(18.1622303, 49.8356781),
# Waypoint(18.1624181, 49.8356175),
# Waypoint(49.8355103, 18.1625603),
# Waypoint(49.8354083, 18.1626969),
# Waypoint(49.8353356, 18.1627667),
# Waypoint(49.8351281, 18.1626489)
])
self.pt.disable_rotation()
copy = None
while True:
self.update_position()
self.vis.update()
if self.waypoints_changed:
with self.waypoints_lock:
copy = self.waypoints[:]
self.waypoints_changed = False
self.planner.plan(copy)
self.pt.enable_rotation()
time.sleep(0.1)
def follow_qr(self):
while True:
self.planner.plan_from_camera()
self.update_position()
time.sleep(0.2)
self.vis.update()
