Skip to content
Snippets Groups Projects
Select Git revision
  • 76a94216f55926a316d77e6b7cc947ea1d7b508c
  • main default protected
  • Localisation
3 results

game.py

Blame
  • game.py 8.53 KiB
    import pygame
    from track import *
    from car_drawer import CarDrawer
    from car_model import Car
    from input_providers import *
    from car_data_display import CarDataDisplay
    from pygame.math import Vector2
    import json
    
    from shapely.geometry import Point
    from random import randint
    
    class Game:
        def __init__(self, width, height):
            pygame.init()
            pygame.display.set_caption("Car model")
            self.window_width = width
            self.window_height = height
            self.screen = pygame.display.set_mode((width, height))
            self.fps = 1
            self.background = Background('BlueCheckerPatternPaper.png', [-500, -500])
    
            self.clock = pygame.time.Clock()
            self.ticks = 20
            self.exit = False
            self.x, self.y = [], []
            self.time = []
    
        def run(self):
    
            car = Car(self.window_width / 20, self.window_height / 20)
            track = Track('track3.svg')
            track_drawer = TrackDrawer(track)
            car.position.x, car.position.y = track.full_path[0][0] / 10 - 1366 / 20, track.full_path[0][1] / 10 - 768 / 20
            car_drawer = CarDrawer()
            input_provider = JoystickInputProvider()
            if not input_provider.joysticks:
                input_provider = KeyboardInputProvider()
            car_data_display = CarDataDisplay(car)
            trace = [(car.position.x * 10 + 1366/2, car.position.y * 10 + 768/2) for _ in range(3)]
    
            while not self.exit:
                dt = self.clock.tick(self.fps) / 1000
    
                # Event queue
                for event in pygame.event.get():
                    if event.type == pygame.QUIT:
                        self.exit = True
    
                trace.pop(2)
                trace.insert(0, (car.position.x * 10 + 1366/2, car.position.y * 10 + 768/2))
    
                # User input
                car_input = input_provider.get_input()
                car.get_driver_input(car_input[0], car_input[1], car_input[2], car_input[3])
                car.update(dt)
    
                # Drawing
                self.screen.fill((0, 0, 0))
                track_drawer.draw(self.screen, car.position * 10, trace)
                car_drawer.draw(self.screen, car)
                car_data_display.display_data(self.screen)
                pygame.display.flip()
            pygame.quit()
    
        def run_pid_controller(self, track_path, solution_path='solutionOpt.csv', dt=10):
            car = Car(self.window_width / 20, self.window_height / 20, angle=90)
            track = Track(track_path)
            track_drawer = TrackDrawer(track)
            solution = pd.read_csv(solution_path, index_col=0)
            track.apply_deformations(list(solution.Deformation))
            car.position.x, car.position.y = track.full_path[0][0] / 10 - 1366 / 20, track.full_path[0][1] / 10 - 768 / 20
            car_drawer = CarDrawer()
            car_data_display = CarDataDisplay(car, track)
            input_provider = AutonomousDriver(solution)
            time = 0
            trace = [(car.position.x * 10 + 1366/2, car.position.y * 10 + 768/2) for _ in range(3)]
            last_u = -1
            while not self.exit:
                """fps perturbation"""
                fps = 20
                u_dt = 1/fps
                
                # Handling time
                self.clock.tick(1 / dt)
                time += dt
    
                # Event queue
                for event in pygame.event.get():
                    if event.type == pygame.QUIT:
                        self.exit = True
    
                # Input from solution file and PID controller
                input_provider.index = track_drawer.chunk_indexes[-1] # Incrementing row index of solution matrix
                car_input = input_provider.get_input(track.track_phase)
                
                turning=car_input[3]
                
                """perturbations"""
                incert_turning = 0 #5°
                turning += randint(-incert_turning, incert_turning)
                
                """limit turning"""
                max_steering = 30
                if turning<0:
                    turning = max(turning, -max_steering)
                else:
                    turning= min(turning, max_steering)
                
                if abs(time-last_u)>u_dt:
                    last_u=time
                    car.get_driver_input(car_input[0], car_input[1], car_input[2], turning)
                car.update(dt)
    
                # Calculating position of a front center of a car
                vector = Vector2(40, 0).rotate(-car.angle)
                vector = np.array((vector.x + 1366 / 2, vector.y + 768 / 2))
                front_center = Point(np.array((car.position.x * 10, car.position.y * 10)) + vector)
    
    
                """perturbations"""
                R_simu = 150
                R_reel = 8
                incert_reel = 0 #0.2m
                incert_simu = incert_reel*R_simu/R_reel
                
                incert_simu = int(incert_simu)
    
                error_x = randint(-incert_simu, incert_simu)
                error_y = randint(-incert_simu, incert_simu)
                error_point = Point(front_center.x+error_x, front_center.y+error_y)
                
                # Calculating line error as a distance from front center to a given line (path)
                i = 1# check if mid is past, if yes, track is on the left so we need to adapt
                if track.is_starting or track.is_ending:
                    input_provider.line_error = track.mid_track-error_point.x
                else:
                    if not track.is_right:
                        i=-1
                    e = LineString(track.visible_path).distance(error_point)
                    #print(e)
                    if Polygon(track.visible_path).contains(error_point):
                        input_provider.line_error = - i * e
                    else:
                        input_provider.line_error = i * e
                        
                self.x.append(front_center.x)
                self.y.append(front_center.y)
                self.time.append(time)
                
                # Updating trace
                trace.pop(2)
                trace.insert(0, (car.position.x * 10 + 1366 / 2, car.position.y * 10 + 768 / 2))
    
                # Drawing
                self.screen.fill((0, 0, 0))
                self.background.set_location([- car.position.x * 10, 768 - car.position.y * 10])
                self.screen.blit(self.background.image, self.background.rect)
                self.background.set_location([1200 - car.position.x * 10, 768 - car.position.y * 10])
                self.screen.blit(self.background.image, self.background.rect)
                self.background.set_location([2400 - car.position.x * 10, 768 - car.position.y * 10])
                self.screen.blit(self.background.image, self.background.rect)
                self.background.set_location([- car.position.x * 10, 768 + 1200 - car.position.y * 10])
                self.screen.blit(self.background.image, self.background.rect)
                self.background.set_location([1200 - car.position.x * 10, 768 + 1200 - car.position.y * 10])
                self.screen.blit(self.background.image, self.background.rect)
                self.background.set_location([2400 - car.position.x * 10, 768 + 1200 - car.position.y * 10])
                self.screen.blit(self.background.image, self.background.rect)
                self.background.set_location([- car.position.x * 10, 768 + 2400 - car.position.y * 10])
                self.screen.blit(self.background.image, self.background.rect)
                self.background.set_location([1200 - car.position.x * 10, 768 + 2400 - car.position.y * 10])
                self.screen.blit(self.background.image, self.background.rect)
                self.background.set_location([2400 - car.position.x * 10, 768 + 2400 - car.position.y * 10])
                self.screen.blit(self.background.image, self.background.rect)
                track_drawer.draw(self.screen, car.position * 10, trace)
                car_drawer.draw(self.screen, car)
                car_data_display.display_data(self.screen)
                rect = pygame.Rect(front_center.x - car.position.x * 10, front_center.y - car.position.y * 10, 5, 5)
                pygame.draw.rect(self.screen, (0, 0, 255), rect)
                pygame.display.flip()
                
                #check if track is done
                completed = track.max_y<front_center.y
                if completed:
                    break
                # Checking if car has passed finishing-line
                if input_provider.index == len(track.track_chunks) - 1:
                    break
                
                
            # Checking if car completed track
            # completed = True
            # for chunk in track.track_chunks:
            #     if not chunk.is_active:
            #         completed = False
            #         break
    
            print("Time:", time, "Completed:", completed)
            data = {"time":self.time, "x":self.x, "y":self.y}
            """saving data"""
            """
            with open('error.json', 'w') as outfile:
                json.dump(data, outfile, indent=4)
            print('SAVED')
            """
            pygame.quit()