Merge branch 'main' of gitlab.ec-lyon.fr:epsa-personnal-git/epsa-pae-ai-self-driving

Documentation/Projet/Simulation/2D-car-dynamics-simulation-master/etude_error.txt

+dimensions circuits:
+Simulation:
+	centre: x=1725, y=
+	sommet gauche: x=1632, y=-832
+	bas gauche: x=1565, y=-505
+	sommet droit: x=1855, y=-830
+	bas droit: x=1925, y=-512
+	droite droite: x=2040, y=-675
+	gauche gauche: x=1420 , y=-700
+
+	=> R/2 = -93, -160, 130, 200
+	=> R = 327, 318, 315, 305
+
+	Rmoy = 300
+	

Documentation/Projet/Simulation/2D-car-dynamics-simulation-master/game.py

+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()

Documentation/Projet/Simulation/2D-car-dynamics-simulation-master/pid_controller.py

+import numpy as np
+
+
+class PidController:
+    def __init__(self, p_gain, i_gain, d_gain, set_point=0):
+        self.p_gain = p_gain
+        self.i_gain = i_gain
+        self.d_gain = d_gain
+        self.set_point = set_point
+        self.integrated_error = 0
+        self.previous_error = 0
+
+    def get_control(self, process_value):
+        error = self.set_point - process_value
+        control = self.p_gain * error + self.i_gain * self.integrated_error + self.d_gain * (error - self.previous_error)
+        self.previous_error = error
+        self.integrated_error += error
+        return np.sign(control) * abs(control)

Documentation/Projet/Simulation/2D-car-dynamics-simulation-master/run.py

+from input_providers import *
+from tqdm import tqdm
+from tabu_search import Search, PointSolution
+from simulator import Simulator
+from game import Game
+
+
+if __name__ == '__main__':
+    game = Game(1366, 768)
+    game.run_pid_controller('track_11.svg', 'test_solution.csv', 0.05)
+    #game.run()
+    #sim = Simulator('track6.svg')
+    #solution = pd.read_csv('solutionOpt.csv', index_col=0)
+    #print(sim.run(0.05, solution))
+