diff --git a/.gitignore b/.gitignore index ec323d5..b88663a 100644 --- a/.gitignore +++ b/.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 diff --git a/docs/_posts/2024-03-07-Semana-17.md b/docs/_posts/2024-03-07-Semana-17.md new file mode 100644 index 0000000..c92436f --- /dev/null +++ b/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)] +``` + +
+ +
Distribución Dataset
+
+ +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. + + +
+ +
Frecuencias piloto experto
+
+ +
+ +
Frecuencias piloto neuronal
+
+ + +
+ +
Result
+
+ +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. diff --git a/docs/assets/images/post17/dataDistribution.png b/docs/assets/images/post17/dataDistribution.png new file mode 100644 index 0000000..4a6e869 Binary files /dev/null and b/docs/assets/images/post17/dataDistribution.png differ diff --git a/docs/assets/images/post17/expertFrq.png b/docs/assets/images/post17/expertFrq.png new file mode 100644 index 0000000..23d6d22 Binary files /dev/null and b/docs/assets/images/post17/expertFrq.png differ diff --git a/docs/assets/images/post17/neuralFreq.png b/docs/assets/images/post17/neuralFreq.png new file mode 100644 index 0000000..492d91a Binary files /dev/null and b/docs/assets/images/post17/neuralFreq.png differ diff --git a/docs/assets/images/post17/result.png b/docs/assets/images/post17/result.png new file mode 100644 index 0000000..64759c3 Binary files /dev/null and b/docs/assets/images/post17/result.png differ diff --git a/src/drone_driver/include/data.py b/src/drone_driver/include/data.py index 9af0a30..a12bc58 100755 --- a/src/drone_driver/include/data.py +++ b/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,7 +76,7 @@ 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: @@ -79,6 +84,8 @@ def get_vels(folder_path): 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() \ No newline at end of file +]) \ No newline at end of file diff --git a/src/drone_driver/src/droneNeuralPilot.py b/src/drone_driver/src/droneNeuralPilot.py index ba92554..d94e567 100755 --- a/src/drone_driver/src/droneNeuralPilot.py +++ b/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()) diff --git a/src/drone_driver/src/plotFunctions/datasetGraphic.py b/src/drone_driver/src/plotFunctions/datasetGraphic.py index 1e9438f..ccba822 100644 --- a/src/drone_driver/src/plotFunctions/datasetGraphic.py +++ b/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") diff --git a/src/drone_driver/src/plotFunctions/graphPath.py b/src/drone_driver/src/plotFunctions/graphPath.py index b2743b2..c45744d 100755 --- a/src/drone_driver/src/plotFunctions/graphPath.py +++ b/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,8 +135,10 @@ 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__": @@ -142,6 +146,7 @@ def main(perfectPath, expertPath, neuralPath): 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) \ No newline at end of file + main(args.perfect_path, args.expert_path, args.neural_path, args.file_name) \ No newline at end of file diff --git a/src/drone_driver/src/plotFunctions/plotFrecuency.py b/src/drone_driver/src/plotFunctions/plotFrecuency.py index 271ffe5..0ade24a 100755 --- a/src/drone_driver/src/plotFunctions/plotFrecuency.py +++ b/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() diff --git a/src/drone_driver/src/train.py b/src/drone_driver/src/train.py index 0fcc4e7..3e16219 100755 --- a/src/drone_driver/src/train.py +++ b/src/drone_driver/src/train.py @@ -20,12 +20,14 @@ writer = SummaryWriter() + + BATCH_SIZE = 100 LEARNING_RATE=1e-4 MOMENT=0.05 -TARGET_LOSS = 0.05 -TARGET_CONECUTIVE_LOSS = 4 +TARGET_LOSS = 0.005#0.05 +TARGET_CONSECUTIVE_LOSS = 4 def should_resume(): return "--resume" in sys.argv or "-r" in sys.argv @@ -63,7 +65,7 @@ def train(checkpointPath, datasetPath, model: pilotNet, optimizer: optim.Optimiz targetLoss = TARGET_LOSS epoch = 0 - while consecutiveEpochs != TARGET_CONECUTIVE_LOSS: + while consecutiveEpochs != TARGET_CONSECUTIVE_LOSS: epoch_loss = 0.0 for i, data in enumerate(train_loader, 0): diff --git a/src/drone_driver/utils/checkNeuralFolder.sh b/src/drone_driver/utils/checkNeuralFolder.sh new file mode 100755 index 0000000..ad95bea --- /dev/null +++ b/src/drone_driver/utils/checkNeuralFolder.sh @@ -0,0 +1,71 @@ +#!/bin/bash + +# Gets the arguments +output_directory=$1 +model_folder=$2 + +testTime=180 + +if [ -z "$output_directory" ] || [ -z "$model_folder" ]; then + echo "Usage: $0 " + exit 1 +fi + +test_worlds=("nurburgring_line") +orientations=(0.0) + +tar_files=$(ls "$model_folder" | grep '\.tar$') + +for filename in $tar_files; do + # Remove the .tar extension and add the filename to the array + tar_files_no_extension+=("${filename%.tar}") +done + +tmux new-window -n drone_test +# Split the window into four panes +tmux split-window -v +tmux split-window -v +tmux split-window -v + +iteration=0 +mkdir -p $output_directory/results + +for name in "${tar_files_no_extension[@]}"; do + for world in "${test_worlds[@]}"; do + + # Executes the simulator + tmux send-keys -t 0 "ros2 launch drone_driver as2_sim_circuit.launch.py world:=../worlds/$world.world yaw:=0.0" C-m + sleep 5 + + tmux send-keys -t 1 "ros2 launch drone_driver neuralPilot.launch.py trace:=False out_dir:=$output_directory/profiling/$world$iteration network_path:=$model_folder/${name}.tar | tee temp_output.txt" C-m + + # Waits until the drone is flying + while ! grep -q "Following" temp_output.txt; do + sleep 1 + done + rm temp_output.txt + sleep 2 + + # Starts recording the topics + tmux send-keys -t 2 "ros2 bag record -o $output_directory/test$name /tf /drone0/sensor_measurements/frontal_camera/image_raw /drone0/self_localization/twist" C-m + sleep "$testTime" + + # Sends ctrl+C to end the simulation + tmux send-keys -t 0 C-c + tmux send-keys -t 1 C-c + tmux send-keys -t 2 C-c + + sleep 2 + iteration=$((iteration + 1)) + + # Saves the results image + tmux send-keys -t 3 "python3 ../src/plotFunctions/graphPath.py --perfect_path ../src/plotFunctions/positionRecords/perfectNurburGingTF/ --expert_path ../src/plotFunctions/positionRecords/EPNumbirngTF --neural_path $output_directory/test$name --file_name "$output_directory/results/$name.png"" C-m + + done +done + +# Kills the other panels +tmux kill-pane -t 1 +tmux kill-pane -t 2 +tmux kill-window -t drone_test + diff --git a/src/drone_driver/utils/generateDataset.sh b/src/drone_driver/utils/generateDataset.sh index a69d9b1..4fa9f41 100755 --- a/src/drone_driver/utils/generateDataset.sh +++ b/src/drone_driver/utils/generateDataset.sh @@ -9,7 +9,7 @@ if [ -z "$record_time" ] || [ -z "$output_directory" ]; then exit 1 fi -worlds=( "simple_circuit" "montmelo_line" "nurburgring_line") +worlds=( "simple_circuit" "montmelo_line" "montreal_line") orientations=(0.0 3.14) tmux new-window -n drone_recorder diff --git a/src/drone_driver/utils/netCheckpointSaver.sh b/src/drone_driver/utils/netCheckpointSaver.sh new file mode 100755 index 0000000..5ed11e5 --- /dev/null +++ b/src/drone_driver/utils/netCheckpointSaver.sh @@ -0,0 +1,20 @@ +#!/bin/bash + +# Gets the arguents +network_path=$1 + +iteration=0 + +if [ -z "$network_path" ]; then + echo "Usage: $0 " + exit 1 +fi + +while true; do + new_network_path="${network_path%.tar}-$iteration.tar" + + cp "$network_path" "$new_network_path" + + sleep 3600 + ((iteration++)) +done diff --git a/src/drone_driver/utils/trainAndCheck.sh b/src/drone_driver/utils/trainAndCheck.sh index 73f6b05..8b7cb66 100755 --- a/src/drone_driver/utils/trainAndCheck.sh +++ b/src/drone_driver/utils/trainAndCheck.sh @@ -5,10 +5,10 @@ dataset_dir=$1 output_directory=$2 model_name=$3 -testTime=120 +testTime=240 -if [ -z "$dataset_dir" ] || [ -z "$output_directory" ]; then - echo "Usage: $0 " +if [ -z "$dataset_dir" ] || [ -z "$output_directory" ] || [ -z "$model_name" ]; then + echo "Usage: $0 " exit 1 fi @@ -16,11 +16,11 @@ test_worlds=("nurburgring_line") orientations=(0.0) pilots=( "ros2 launch drone_driver neuralPilot.launch.py trace:=False out_dir:=$output_directory/profiling/$world$iteration network_path:=$model_name" - "ros2 launch drone_driver expertPilot.launch.py trace:=False out_dir:=$output_directory/profiling/$world$iteration" + # "ros2 launch drone_driver expertPilot.launch.py trace:=False out_dir:=$output_directory/profiling/$world$iteration" ) names=("Neural" "Expert") -python3 ../src/train.py --dataset_path $dataset_dir --network_path $output_directory +#python3 ../src/train.py --dataset_path $dataset_dir --network_path $output_directory tmux new-window -n drone_test