utils update

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

 import xml.etree.ElementTree as ET
-from track_2_generator import create_track, create_double_circles
+from track_2_generator import create_track
 from shapely.geometry import Polygon, Point, LineString
 import numpy as np
 

PAR 152/Yolo V3/TensorFlow-2.x-YOLOv3-master/yolov3/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)

PAR 152/Yolo V3/TensorFlow-2.x-YOLOv3-master/yolov3/track_2_generator.py

+from math import cos, sin, pi
+def create_track(r, start_point_distance_y, end_point_distance_y, tot_shift):
+    laps = 1 #nombre de tours
+    resolution_circle = 10 #resolution du cercle en degre
+    resolution_line = 10
+    
+    #contruire les points qui vont former le chemin
+    points = [(0, start_point_distance_y)]
+    current_x, current_y = 0, start_point_distance_y
+    if current_y<0:
+        #faire les points de la première ligne droite
+        while current_y<0:
+            current_y += resolution_line
+            points.append((current_x, current_y))
+    
+    #faire les points des cercles
+    current_angle = 0 #angle en degree
+    for i in range(laps):
+        while current_angle<720:
+            rad = current_angle/360*2*pi
+            if 0<current_angle<360:
+                current_x, current_y = r-r*cos(rad), r*sin(rad)
+            else:
+                current_x, current_y = -(r-r*cos(rad)), r*sin(rad)
+            current_angle+=resolution_circle
+            points.append((current_x, current_y))
+        
+    if current_y<end_point_distance_y:
+        current_x=0
+        while current_y<end_point_distance_y:
+            current_y += resolution_line
+            points.append((current_x, current_y))
+    
+    
+    #on decale les points 
+    shift_x = -min([x[0] for x in points])
+    shift_y = -min([x[1] for x in points])
+    
+    points = [(x+shift_x+tot_shift[0],y+shift_y+tot_shift[1]) for x,y in points]
+    
+    #print(points)
+    return points
+    
+def create_double_circles(r, left_center, right_center):
+    resolution_circle = 10 #resolution du cercle en degre
+    current_angle=0
+    l_l = []
+    l_r = []
+    while current_angle<360:
+        rad = current_angle/360*2*pi
+        current_x_left, current_y_left = left_center[0]+r*cos(rad), left_center[1]+r*sin(rad)
+        current_x_right, current_y_right = right_center[0]+r*cos(rad), right_center[1]+r*sin(rad)
+        #if current_x_left<(left_center[0]+right_center[0])/2:
+        l_l.append((current_x_left, current_y_left))
+        #if current_x_right>(left_center[0]+right_center[0])/2:
+        l_r.append((current_x_right, current_y_right))
+        current_angle+=resolution_circle
+        
+    return l_l+l_r
+
+if __name__ =="__main__":
+    points = create_track(1, -20, 20)
+    #construire le svg
+    mid = ""
+    #prendre le code autour du chemin
+    f = open("basic_track.svg", "r")
+    t = f.read().split('CUT_HERE')
+    for i,point in enumerate(points):
+        mid += f'<circle \n id="path{i}" \n style="fill:#000000;stroke:none" \n cx="{point[0]}" \n cy="{point[1]}" \n r="0.1" /> \n'
+    t = t[0]+mid+t[1]
+    
+    f = open("track_11.svg", "w")
+    f.write(t)
+    f.close()
+    
\ No newline at end of file

PAR 152/Yolo V3/TensorFlow-2.x-YOLOv3-master/yolov3/utils.py

@@ -18,6 +18,9 @@ import tensorflow as tf
 from yolov3.configs import *
 from yolov3.yolov4 import *
 from tensorflow.python.saved_model import tag_constants
+from track_2_generator import create_track
+from pid_controller import PidController
+from shapely.geometry import Polygon, Point, LineString
 
 def load_yolo_weights(model, weights_file):
     tf.keras.backend.clear_session() # used to reset layer names
@@ -153,6 +156,12 @@ def draw_bbox(image, bboxes, CLASSES=YOLO_COCO_CLASSES, show_label=True, show_co
         # put object rectangle
         cv2.rectangle(image, (x1, y1), (x2, y2), bbox_color, bbox_thick*2)
         
+        print((x1, y1),(x2, y2))
+
+        """Determinate position"""
+        
+        
+        
         if show_label:
             # get text label
             score_str = " {:.2f}".format(score) if show_confidence else ""
@@ -175,6 +184,25 @@ def draw_bbox(image, bboxes, CLASSES=YOLO_COCO_CLASSES, show_label=True, show_co
             cv2.putText(image, label, (x1, y1-4), cv2.FONT_HERSHEY_COMPLEX_SMALL,
                         fontScale, Text_colors, bbox_thick, lineType=cv2.LINE_AA)
             
+            
+    """Algorithme de conduite"""
+    """x_car, y_car"""
+    
+    """loading trajectory"""#TODO: load it only once
+    track_points = create_track(0.5,0,0, (0,0))[:-1]+[(1,0.5)]
+    track = Polygon(track_points)
+    
+    """calculate distance"""
+    front_center = Point(np.array([x_car,y_car]))
+    error = track.distance(front_center)
+    
+    """calculate PID response"""
+    pid = PidController(1,1,1) #P, I, D
+    commande = pid.get_control(error)
+    
+    """sending info to car"""
+    
+
     return image
 
 

PAR 152/carte_cones.py

+pixel_x_ext, pixel_y_ext = [242, 220, 165, 110, 63, 33, 22, 34, 63, 110, 165, 220, 243, 310, 334, 388, 443, 490, 521, 531, 520, 489, 443, 388, 333, 310], [76, 64, 52, 64, 95, 141, 196, 252, 298, 330, 340, 328, 318, 316, 328, 339, 329, 298, 251, 196, 142, 95, 64, 53, 64, 77]
+pixel_x_int, pixel_y_int = [245, 238, 222, 196, 166, 134, 108, 91, 85, 90, 109, 134, 165, 196, 222, 239, 308, 314, 332, 358, 388, 419, 445, 462, 468, 462, 445, 419, 388, 359, 332, 314], [201, 167, 140, 123, 116, 123, 140, 165, 195, 228, 253, 270, 277, 270, 253, 227, 200, 226, 253, 270, 277, 270, 253, 228, 197, 166, 140, 122, 117, 123, 140, 166]
+diametre = 225
+centre_x, centre_y = 278, 200
+coord_x_ext, coord_y_ext = [i/diametre for i in pixel_x_ext], [i/diametre for i in pixel_y_ext]
+coord_x_int, coord_y_int = [i/diametre for i in pixel_x_int], [i/diametre for i in pixel_y_int]
+print(coord_x_int, coord_x_int)
\ No newline at end of file

PAR 152/positionnement.py

@@ -134,16 +134,25 @@ def positionnement(new_pos_cone, liste_ind_cone):
 if __name__ == '__main__':
     
     #initialisation
+    car_pos = (0,0)
+    car_angle = 0
     pos = [0,0,0]
     old_pos_cone = []
     ind_cone = []
     
     #boucle
-    pos_cone = []
+    pos_cones = []
     for i in boxes:
-        pos_cone = boite2coord(i[0],i[1],i[2],i[3])
-    pos, liste_ind_cone = identification_cones(pos, old_pos_cone, pos_cone, ind_cone)
-    old_pos_cone = pos_cone
+        pos_cones.append(boite2coord(i[0],i[1],i[2],i[3]))
+        
+    pos2=[]
+    for c in pos_cones:
+        pos2.append((car_pos[0]+np.cos(car_angle)*c[0]+np.sin(car_angle)*c[1], car_pos[1]+np.cos(car_angle)*c[1]-np.sin(car_angle)*c[0]))
+    print(pos2)
+    
+    
+    #pos, liste_ind_cone = identification_cones(pos, old_pos_cone, pos_cone, ind_cone)
+    #old_pos_cone = pos_cone
     
     
             
\ No newline at end of file