Semana 17

.gitignore

@@ -5,3 +5,4 @@ src/drone_driver/src/car
 src/drone_driver/profiling
 src/drone_driver/utils/*.tar
 src/drone_driver/src/plotFunctions/positionRecords
+src/drone_driver/networks

docs/_posts/2024-03-07-Semana-17.md

@@ -0,0 +1,86 @@
+---
+title: "Semana 17 - Piloto neuronal!!!"
+categories:
+  - Registro Semanal
+tags:
+  - ROS2
+  - Aprendizaje Automático
+---
+
+## Índice
+
+---
+
+## Fallo en la aumentación
+
+Lo primero que se descubrió es que en nuestro caso la aumentación no nos favorecía tanto, ya que al hacer que el aprendizaje no dependiera del color, se detectaba la pared del circuito como la línea, por lo que se tuvo que quitar esta característica.
+
+---
+
+## Cambio de enfoque
+
+Tras una prueba, me di cuenta de que no sabía a ciencia cierta qué umbral de `mse_loss` era el más indicado para finalizar el entrenamiento, por lo que se desarrolló otra manera de probar los modelos neuronales. Se ejecutó un entrenamiento de manera normal con el comando:
+
+```python
+python3 train.py --dataset_path ../training_dataset/expertPilot_VX/ --network_path ../networks/netX/netX.tar
+```
+
+Entonces se creó un script que cada hora fuera copiando la red neuronal, guardando el estado de la misma con el avance del entrenamiento durante un tiempo indefinido. Se dejó esta red entrenando bastantes horas, dejando una carpeta con bastantes modelos.
+
+El siguiente paso fue la automatización de la comprobación. Para esto se recorrió esta misma carpeta ejecutando cada modelo; el resultado se guardó como una imagen. Esto permitía ver con claridad qué modelos no funcionaban. Analizando los resultados se seleccionaron las gráficas donde menor error y tiempo por vuelta salía como resultado.
+
+---
+---
+
+## Cambios en el dataset
+
+Se trataron varias distribuciones con las variables de pesos, la que dio un mejor resultado fue la siguiente
+
+```python3
+weights = [(0.85, 0.15, 0.25),     
+            (0.65, 0.55, 0.45),    
+            (0.35, 0.75, 0.995),   
+            (0.35, 0.75, 0.995),   
+            (0.65, 0.55, 0.45),     
+            (0.85, 0.15, 0.25)]      
+```
+
+<figure class="align-center" style="width:60%">
+  <img src="{{ site.url }}{{ site.baseurl }}/assets/images/post17/dataDistribution.png" alt="">
+  <figcaption>Distribución Dataset</figcaption>
+</figure>
+
+También se distribuyeron de varias maneras las velocidades a la hora de entrenar. Leí una técnica que consistía en aumentar el valor de la variable que más importara en la estabilidad del sistema. Esto se tradujo en multiplicar la velocidad angular por 3 para que así tuviera un mayor peso y el error en la misma se corrigiera más rápido. Después de la inferencia se dividía su valor de nuevo entre 3 para que el drone se comportara adecuadamente.
+
+En un principio dio buenos resultados, pero la velocidad lineal no se ajustaba mucho, por lo que se decidió normalizar ambas con sus valores máximos y mínimos y después normalizar.
+
+## Video final
+
+Tras varias pruebas se consiguió que el piloto neuronal pasara el circuito de pruebas, pero quedaba un pequeño problema. El drone no era tan estable como se esperaba. Tras releer varios papeles de semanas anteriores, me di cuenta de que una de las características de estas redes neuronales es que no tienen memoria ni retroalimentación. En nuestro caso, lo más parecido y sencillo de implementar con nuestro modelo sería que la entrada del mismo fuera de 4 imágenes, pero no daría resultados muy distintos.
+
+Pensándolo, nuestra mayor dependencia era que si el cambio de velocidad lineal era muy brusco, haría al drone cabecear y esto produciría un movimiento de cámara que desestabilizaría al drone. Para solucionar esto se usó la misma medida que en el piloto experto: hacer una media de x velocidades lineales para calcular la actual.
+
+Con esta técnica se respeta la inferencia de la red neuronal y aportó una mucho mejor estabilidad, dando el siguiente resultado:
+
+## Conclusiones de la semana
+
+No siempre más entrenamiento significa mejores resultados. Esto creo que podría ser debido a un sobreajuste, y hay veces que las redes neuronales tienen límites como es el caso de la "memoria" o retroalimentación de velocidades mencionadas anteriormente. Sin embargo, aporta la mejora de que su frecuencia de decisión es mucho más alta que la del piloto experto, pudiendo tener un mejor tiempo de reacción.
+
+<!-- Foto frecuencias piloto experto -->
+<figure class="align-center" style="width:60%">
+  <img src="{{ site.url }}{{ site.baseurl }}/assets/images/post17/expertFrq.png" alt="">
+  <figcaption>Frecuencias piloto experto</figcaption>
+</figure>
+<!-- Foto frecuencias piloto neuronal -->
+<figure class="align-center" style="width:60%">
+  <img src="{{ site.url }}{{ site.baseurl }}/assets/images/post17/expertFrq.png" alt="">
+  <figcaption>Frecuencias piloto neuronal</figcaption>
+</figure>
+
+<!-- Mapa con resultado de ambas rutas -->
+<figure class="align-center" style="width:60%">
+  <img src="{{ site.url }}{{ site.baseurl }}/assets/images/post17/result.png" alt="">
+  <figcaption>Result</figcaption>
+</figure>
+
+Podemos ver la diferencia de media entre frecuencias del piloto experto, con una media de 125 Hz, al piloto neuronal que, aunque tenga más desviación, su media ronda al doble. Respecto a resultados prácticos, es cierto que el piloto neuronal tiene un poco más de error y tarda 5 segundos más en completar el circuito.

src/drone_driver/include/data.py

@@ -14,13 +14,18 @@
 import os
 from PIL import Image
 
+# General velocity's
+MAX_ANGULAR = 2.0
+MAX_LINEAR = 6.3
+MIN_LINEAR = 3.0
+
+# Labels
 ANGULAR_UMBRALS = [-0.5, -0.2, 0, 0.2, 0.5, float('inf')]  # label < umbral
 LINEAR_UMBRALS = [4.5, 5.5, float('inf')]
 
-DATA_PATH = "../training_dataset"
-
-USE_WEIGHTS = True       # 0 false   1 false    2 True
-USE_AUGMENTATION = True  # 0 false   1 true     3 True
+# General aspects
+USE_WEIGHTS = True
+USE_AUGMENTATION = False
 
 def get_image_dataset(folder_path):
     images = []
@@ -71,14 +76,16 @@ def get_vels(folder_path):
             with open(file_path, 'r') as file:
                 content = file.read()
                 numbers = [float(num) for num in content.split(',')]
-
+        
                 vels.append([numbers[0], numbers[1]*3])
 
     except FileNotFoundError:
         print("Error: File not found.")
 
     return vels
 
+
+
 class rosbagDataset(Dataset):
     def __init__(self, main_dir, transform, boolAug=USE_AUGMENTATION, dataAugment=1) -> None:
         self.main_dir = main_dir
@@ -109,22 +116,25 @@ def get_dataset(self):
         return dataset
 
 
-
     def __getitem__(self, item):
         device = torch.device("cuda:0")
 
+        angular = (self.dataset[item][1][1] + MAX_ANGULAR) / (2 * MAX_ANGULAR)
+        lineal = (self.dataset[item][1][0] + MIN_LINEAR) / (MAX_LINEAR - MIN_LINEAR)
+        norm = (lineal, angular)
+
         # Apply augmentation
         if self.applyAug:
             image_tensor = torch.tensor(self.dataset[item][0]).to(device)
-            image_tensor, vel_tensor = self.applyAugmentation(device, image_tensor, self.dataset[item][1])
+            image_tensor, vel_tensor = self.applyAugmentation(device, image_tensor, norm)
 
         # Not apply augmentation
         else:
             image_tensor = self.transform(self.dataset[item][0]).to(device)
-            vel_tensor = torch.tensor(self.dataset[item][1]).to(device)
+            vel_tensor = torch.tensor(norm).to(device)
 
-        return (vel_tensor, image_tensor)
-
+            
+        return (vel_tensor, image_tensor)    
 
     def applyAugmentation(self, device, imgTensor, velocityValue):
         mov = 2
@@ -192,14 +202,20 @@ def getAngularSubset(self, dataset, lowerBound, upperBound):
 
     def balancedDataset(self):
         useWeights = USE_WEIGHTS
-        #   <       5     5.5   inf
-        weights = [(0.99, 0.3, 0.2),     # < -0.6
-                   (0.80, 0.8, 0.4),     # < -0.3
-                   (0.60, 0.7, 0.99),    # < 0
-                   (0.60, 0.7, 0.99),    # < 0.3
-                   (0.80, 0.8, 0.4),     # < 0.6
-                   (0.99, 0.3, 0.2)]      # < inf
-
+        # #   <       5     5.5   inf       Weights 1
+        # weights = [(0.99, 0.3, 0.2),     # < -0.6
+        #            (0.80, 0.8, 0.4),     # < -0.3
+        #            (0.60, 0.7, 0.99),    # < 0
+        #            (0.60, 0.7, 0.99),    # < 0.3
+        #            (0.80, 0.8, 0.4),     # < 0.6
+        #            (0.99, 0.3, 0.2)]      # < inf
+
+        weights = [(0.85, 0.15, 0.25),     # < -0.6
+                   (0.65, 0.55, 0.45),     # < -0.3
+                   (0.35, 0.75, 0.995),    # < 0
+                   (0.35, 0.75, 0.995),    # < 0.3
+                   (0.65, 0.55, 0.45),     # < 0.6
+                   (0.85, 0.15, 0.25)]      # < inf
 
         # Gets all the subsets
         labeledDataset = [self.getAngularSubset(self.dataset, float('-inf'), ANGULAR_UMBRALS[0])]
@@ -266,23 +282,4 @@ def balancedDataset(self):
 dataset_transforms = transforms.Compose([
     transforms.ToTensor(),
     transforms.Resize([66, 200]),
-])
-
-
-if __name__ == "__main__":
-
-    # Creates data instance
-    dataset = rosbagDataset(main_dir=DATA_PATH, transform=dataset_transforms, boolAug=True, dataAugment=2)
-
-    # Shows some images with the augmentation 
-    for i in range(15): 
-        idx = random.randint(0, len(dataset) - 1)
-        velocity, image = dataset[idx]
-
-        image = image.permute(1, 2, 0).cpu().numpy()
-        image = (image * 0.5) + 0.5 
-
-        # Shows the image and the associated velocity 
-        plt.imshow(image)
-        plt.title(f"Velocidad: {velocity}")
-        plt.show()
+])

src/drone_driver/src/droneNeuralPilot.py

@@ -23,12 +23,14 @@
 from include.control_functions import PID, save_timestamps, save_profiling
 from include.models import pilotNet
 from train import load_checkpoint
-from include.data import dataset_transforms
+from include.data import dataset_transforms, MAX_ANGULAR, MIN_LINEAR, MAX_LINEAR
 
 # Vel control
 MAX_Z = 2
+LINEAL_ARRAY_LENGTH = 150
+REDUCTION = 3.5
 
-## PID controlers
+## PID controllers
 Z_KP = 0.5
 Z_KD = 0.45
 Z_KI = 0.0
@@ -64,6 +66,8 @@ def __init__(self, drone_id: str = "drone0", verbose: bool = False, use_sim_time
 
         self.cv_image = None
         self.imageTensor = None
+        self.lastVels = []
+        self.lastAngular = []
 
 
         # Folder name
@@ -74,7 +78,7 @@ def __init__(self, drone_id: str = "drone0", verbose: bool = False, use_sim_time
 
     def save_data(self):
 
-        # Creats the directory 
+        # Creates the directory 
         os.makedirs(self.profilingDir, exist_ok=True)
 
         # Saves all the data
@@ -99,7 +103,7 @@ def listener_callback(self, msg):
 
         self.profiling.append(f"Tensor conversion time = {time.time() - initTime}")
 
-    # Retrys aerostack command if it failed
+    # Retry aerostack command if it failed
     def retry_command(self, command, check_func, sleep_time=1.0, max_retries=1):
         if not check_func():
             command()
@@ -148,7 +152,7 @@ def land_process(self, speed):
 
 
     def set_vel2D(self, vx, vy, pz, yaw):
-        # Gets the drone velocitys
+        # Gets the drone velocity's
         velX = vx * math.cos(self.orientation[2]) + vy * math.sin(self.orientation[2])
         velY = vx * math.sin(self.orientation[2]) + vy * math.cos(self.orientation[2])
 
@@ -162,7 +166,7 @@ def set_vel2D(self, vx, vy, pz, yaw):
 
 
     def set_vel(self, vx, vy, vz, yaw):
-        # Gets the drone velocitys
+        # Gets the drone velocity's
         velX = vx * math.cos(self.orientation[2]) + vy * math.sin(self.orientation[2])
         velY = vx * math.sin(self.orientation[2]) + vy * math.cos(self.orientation[2])
 
@@ -179,19 +183,30 @@ def get_vels(self):
             vels = self.model(self.imageTensor)[0].tolist()
 
         self.profiling.append(f"\nAngular inference = {time.time() - initTime}")
-        return vels
+
+        angular = ((vels[1] * (2 * MAX_ANGULAR))  - MAX_ANGULAR) / REDUCTION
+        lineal = ((vels[0] * (MAX_LINEAR - MIN_LINEAR)) - MIN_LINEAR)
+
+        return (lineal, angular)
 
 
     def follow_line(self):
         if self.info['state'] == PlatformStatus.FLYING:
             initTime = time.time()
 
-            # Gets drone velocitys
+            # Gets drone velocity's
             vels = self.get_vels()
+
+            self.lastVels.append(vels[0])
+            if len(self.lastVels) > LINEAL_ARRAY_LENGTH:
+                self.lastVels.pop(0)
+
+            linearVel = np.mean(self.lastVels)
+
             self.get_logger().info("Linear inference = %f  | Angular inference = %f" % (vels[0], vels[1]))
 
             # Set the velocity
-            self.set_vel2D(float(vels[0]), 0, MAX_Z, float(vels[1]/3))
+            self.set_vel2D(float(linearVel), 0, MAX_Z, float(vels[1]))
 
             # Profiling
             self.vel_timestamps.append(time.time())

src/drone_driver/src/plotFunctions/datasetGraphic.py

@@ -34,7 +34,7 @@ def plot_2d(ax, vels):
 
     # Divides angular vels
     for i in range(len(angVel)):
-        dividedVels.append(angVel[i] / 10)
+        dividedVels.append(angVel[i] / 3)
 
     # Sets the labels
     ax.set_xlabel("Linear vel")

src/drone_driver/src/plotFunctions/graphPath.py

@@ -44,8 +44,8 @@ def plot_route(path, label):
 
 def get_lap_time(path):
     # Gets the time between each lap
-    for i in range(int(len(path) / 4)):
-        for j in range(int(len(path) / 4)):
+    for i in range(int(len(path) / 2)):
+        for j in range(int(len(path) / 2)):
 
             if abs(path[i][0] - path[-j][0]) < 0.1 and abs(path[i][1] - path[-j][1]) < 0.1 and abs(i - j) > 20:
 
@@ -79,7 +79,9 @@ def get_lap_error(refPath, path):
     for i in range(len(path)):
         # Corrects the trajectory
         for j in range(slack):
-            ind = (index + j) % len(refPath)
+            ind = (index + j)
+            if ind > len(path):
+                ind = ind - len(path)
 
             # Euclidean distance
             dist = math.sqrt((refPath[ind][0] - path[i][0])**2 + (refPath[ind][1] - path[i][1])**2)
@@ -96,7 +98,7 @@ def get_lap_error(refPath, path):
 
 
 
-def main(perfectPath, expertPath, neuralPath):
+def main(perfectPath, expertPath, neuralPath, fileName):
 
     # Gets the rosbags
     dataset1 = RosbagDataset(perfectPath)
@@ -133,15 +135,18 @@ def main(perfectPath, expertPath, neuralPath):
     plt.title('Results')
     plt.xlabel('x')
     plt.ylabel('y')
+
     plt.legend()
-    plt.show()
+    plt.savefig(fileName)
+    # plt.show()
 
 
 if __name__ == "__main__":
     parser = argparse.ArgumentParser(description='Process ROS bags and plot results.')
     parser.add_argument('--perfect_path', type=str, required=True)
     parser.add_argument('--expert_path', type=str, required=True)
     parser.add_argument('--neural_path', type=str, required=True)
+    parser.add_argument('--file_name', type=str, default="./plot.png")
 
     args = parser.parse_args()
-    main(args.perfect_path, args.expert_path, args.neural_path)
+    main(args.perfect_path, args.expert_path, args.neural_path, args.file_name)

src/drone_driver/src/plotFunctions/plotFrecuency.py

@@ -25,9 +25,9 @@ def plot_frequency(timestamps):
 
 
 def main():
-    data = np.load("./sub_timestamps.npy")
+    #data = np.load("./sub_timestamps.npy")
     data2 = np.load("./vel_timestamps.npy")
-    plot_frequency([data, data2])
+    plot_frequency([data2])
 
 if __name__ == "__main__":
     main()