diff --git a/README.md b/README.md index aaf0627..a02dfea 100644 --- a/README.md +++ b/README.md @@ -10,11 +10,12 @@ Versions Ros Humble Gazebo 11.10.2 --> - + ## Manually + 1. For launch only the world ```bash @@ -30,13 +31,14 @@ ros2 ros2 launch drone_driver expertPilot.launch.py out_dir:=TRACES_DIR trace:=B ## Automation scripts ### Dataset generator + 1. Go to utils directory ```bash cd PATH_TO_WORKSPACE/src/drone_driver/utils ``` -2. Open a tmux sesion +2. Open a tmux session ```bash tmux @@ -47,19 +49,21 @@ tmux ```bash ./generateDataset.sh TIME_RECORDING DATASET_PATH ``` + * **Time recording:** Int -> Time recording the topics of the drone. * **Dataset path** String -> Path where the dataset and profiling data are located.will be save. --- ### Training script + 1. Go to utils directory ```bash cd PATH_TO_WORKSPACE/src/drone_driver/utils ``` -2. Open a tmux sesion +2. Open a tmux session ```bash tmux @@ -73,4 +77,4 @@ tmux * **Dataset path:** String -> Route where the dataset is located. * **Profiling path:** String -> Path where the dataset and profiling data are located. -* **Model path:** String -> Route where the network model will be saved. \ No newline at end of file +* **Model path:** String -> Route where the network model will be saved. diff --git a/docs/_posts/2024-03-07-Semana-16.md b/docs/_posts/2024-03-07-Semana-16.md new file mode 100644 index 0000000..d6dfef5 --- /dev/null +++ b/docs/_posts/2024-03-07-Semana-16.md @@ -0,0 +1,39 @@ +--- +title: "Semana 16 - Entrenamientos finales de simulación" +categories: + - Registro Semanal +tags: + - ROS2 + - Aprendizaje Automático +--- + +## Índice + +- [Índice](#índice) +- [Entrenamiento principal](#entrenamiento-principal) +- [Mejora](#mejora) + +--- + +## Entrenamiento principal + +Se creó otro piloto experto que fuera más estable y rápido. Sin embargo, tras numerosas pruebas, los resultados seguían siendo consistentes, aunque la automatización agilizó significativamente las pruebas. + +Se realizaron varias pruebas: + +1. **Aumento de los augmented labels:** La solución tardaba mucho más en converger, lo que resultaba en un entrenamiento de varias horas sin llegar a una solución óptima. + +2. **Disminución de lr o batch:** No tuvo mucho efecto, pero se observó que si el batch era excesivamente grande, el dron no aprendía adecuadamente. + +3. **Cambio de etiquetas:** Se redistribuyeron de manera más efectiva las etiquetas gracias a los gráficos de las semanas anteriores, lo cual mejoró los resultados, pero no lo suficiente. + +4. **Loss más bajo:** Al finalizar el entrenamiento, se estableció el loss más bajo, ya que no parecía haber sobreajuste y así la red aprendería mejor. + +--- +--- + +## Mejora + +Reflexionando, me di cuenta de que no es del todo óptimo entrenar una red neuronal con un solo piloto experto, ya que lo máximo que podría lograr sería imitarlo perfectamente y seguir sus pasos, pero lo que buscamos es una mejora. + +Por eso mismo se me ocurrió configurar los parámetros de cada piloto experto para que sea óptimo en cada circuito, obtener el dataset de cada piloto experto y posteriormente entrenar la misma red con las imágenes generadas por cada piloto experto. diff --git a/src/drone_driver/include/data.py b/src/drone_driver/include/data.py index 0a1427c..9af0a30 100755 --- a/src/drone_driver/include/data.py +++ b/src/drone_driver/include/data.py @@ -1,8 +1,5 @@ #!/usr/bin/env python3 -# Execute as => python3 train.py --n network2.tar - -from rosbags.rosbag2 import Reader as ROS2Reader import torch import matplotlib.pyplot as plt import random @@ -17,12 +14,13 @@ import os from PIL import Image -ANGULAR_UMBRALS = [-0.3, -0.1, 0, 0.1, 0.3, float('inf')] # label < umbral -LINEAR_UMBRALS = [5, 5.5, float('inf')] +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" -LOWER_LIMIT = 0 -UPPER_LIMIT = 3 + +USE_WEIGHTS = True # 0 false 1 false 2 True +USE_AUGMENTATION = True # 0 false 1 true 3 True def get_image_dataset(folder_path): images = [] @@ -48,7 +46,7 @@ def get_image_dataset(folder_path): images.append(image_array) except FileNotFoundError: - print("Error: Carpeta no encontrada.") + print("Error: File not found.") return images @@ -74,19 +72,19 @@ def get_vels(folder_path): content = file.read() numbers = [float(num) for num in content.split(',')] - vels.append([numbers[0], numbers[1]*10]) + vels.append([numbers[0], numbers[1]*3]) except FileNotFoundError: - print("Error: Carpeta no encontrada.") + print("Error: File not found.") return vels class rosbagDataset(Dataset): - def __init__(self, main_dir, transform, boolAug=False, dataAument=1) -> None: + def __init__(self, main_dir, transform, boolAug=USE_AUGMENTATION, dataAugment=1) -> None: self.main_dir = main_dir self.transform = transform - self.minSamples = 25 - self.dataAument = dataAument + self.minSamples = 10 + self.dataAugment = dataAugment self.imgData = get_image_dataset(main_dir + "/frontal_images") self.velData = get_vels(main_dir + "/labels") @@ -102,7 +100,7 @@ def get_dataset(self): dataset = list(((self.imgData[i], self.velData[i]) for i in range(len(self.velData)))) original_size = len(dataset) - new_size = int(original_size * self.dataAument) + new_size = int(original_size * self.dataAugment) for _ in range(new_size - original_size): randIndex = random.randint(0, original_size - 1) @@ -115,12 +113,12 @@ def get_dataset(self): def __getitem__(self, item): device = torch.device("cuda:0") - # Applys augmentation + # Apply augmentation if self.applyAug: image_tensor = torch.tensor(self.dataset[item][0]).to(device) - image_tensor, vel_tensor = self.applayAugmentation(device, image_tensor, self.dataset[item][1]) + image_tensor, vel_tensor = self.applyAugmentation(device, image_tensor, self.dataset[item][1]) - # Not applys augmentation + # Not apply augmentation else: image_tensor = self.transform(self.dataset[item][0]).to(device) vel_tensor = torch.tensor(self.dataset[item][1]).to(device) @@ -128,51 +126,52 @@ def __getitem__(self, item): return (vel_tensor, image_tensor) - def applayAugmentation(self, device, imageTensor, velocityValue): - mov = 10 + def applyAugmentation(self, device, imgTensor, velocityValue): + mov = 2 + + # Apply color augmentation + augmented_image_tensor = self.colorAugmentation()(image=imgTensor.cpu().numpy())['image'] + imageTensor = torch.tensor(augmented_image_tensor).clone().detach().to(device) - # Aplys color augmentation - augmented_image_tensor = self.colorAugmeentation()(image=imageTensor.cpu().numpy())['image'] - imgTensor = torch.tensor(augmented_image_tensor).to(device) randNum = random.randint(0, 7) # Flips the image so the angular will be in opposite way if randNum == 3 or randNum == 4: # 2 / 8 chance - imgTensor = torch.flip(imgTensor, dims=[-1]) + imageTensor = torch.flip(imageTensor, dims=[-1]) velTensor = torch.tensor((velocityValue[0], -velocityValue[1])).to(device) # Horizontal movement - elif randNum == 2: # 1 / 8 chance + elif randNum == 20: # 1 / 8 chance randMove = random.randint(-mov, mov) - imgTensor = F.affine(imgTensor, angle=0, translate=(randNum, 0), scale=1, shear=0) + imageTensor = F.affine(imageTensor, angle=0, translate=(randNum, 0), scale=1, shear=0) velTensor = torch.tensor((velocityValue[0], velocityValue[1] + randMove/(mov*2))).to(device) # Vertical movement - elif randNum == 1: # 1 / 8 chance + elif randNum == 10: # 1 / 8 chance randMove = random.randint(-mov, mov) - imgTensor = F.affine(imgTensor, angle=0, translate=(0, randMove), scale=1, shear=0) + imageTensor = F.affine(imageTensor, angle=0, translate=(0, randMove), scale=1, shear=0) velTensor = torch.tensor((velocityValue[0] - randMove/(mov*1.5), velocityValue[1])).to(device) else: # No changes # 4 / 8 chance velTensor = torch.tensor(velocityValue).to(device) - return imgTensor, velTensor + finalImgTensor = self.transform(imageTensor.cpu().numpy()).to(device) + return finalImgTensor, velTensor - def colorAugmeentation(self, p=0.5): + def colorAugmentation(self, p=0.5): return A.Compose([ A.HorizontalFlip(p=p), A.RandomBrightnessContrast(), - A.Normalize(), - ToTensorV2(), + A.Normalize() ]) def subSample(self, label, percent): toDel = int(len(label) * percent) - subsampled = random.sample(label, len(label) - toDel) + subsample = random.sample(label, len(label) - toDel) - return subsampled + return subsample def getLinearSubset(self, dataset, lowerBound, upperBound): linearSub = [(img, vel) for img, vel in dataset if lowerBound < vel[0] <= upperBound] @@ -180,7 +179,7 @@ def getLinearSubset(self, dataset, lowerBound, upperBound): return linearSub def getAngularSubset(self, dataset, lowerBound, upperBound): - angularSub = [(img, vel) for img, vel in dataset if lowerBound < vel[1]/10 <= upperBound] + angularSub = [(img, vel) for img, vel in dataset if lowerBound < vel[1]/3 <= upperBound] # Gets the linear subsets angularLabeled = [self.getLinearSubset(angularSub, float('-inf'), LINEAR_UMBRALS[0])] @@ -192,14 +191,14 @@ def getAngularSubset(self, dataset, lowerBound, upperBound): def balancedDataset(self): - useWeights = False + useWeights = USE_WEIGHTS # < 5 5.5 inf - weights = [(0.80, 0.0, 0.0), # < -0.6 - (0.90, 0.7, 0.4), # < -0.3 - (0.50, 0.5, 0.99), # < 0 - (0.50, 0.5, 0.99), # < 0.3 - (0.80, 0.7, 0.4), # < 0.6 - (0.90, 0.0, 0.0)] # < 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 # Gets all the subsets @@ -216,10 +215,10 @@ def balancedDataset(self): if len(linLabeled) > maxSamples: maxSamples = len(linLabeled) - # Auments the data - maxSamples = int(maxSamples * self.dataAument) + # Augments the data + maxSamples = int(maxSamples * self.dataAugment) - # Balances all the clases + # Balances all the classes balancedDataset = [] for i, angLabeled in enumerate(labeledDataset): @@ -231,7 +230,7 @@ def balancedDataset(self): if currentSubsetLen >= self.minSamples: - # Checks if thers enought samples + # Checks if there's enough samples if currentSubsetLen < maxSamples: # If the current length is less than maxSamples, we replicate the elements @@ -243,7 +242,7 @@ def balancedDataset(self): # If the current length is greater than or equal to maxSamples, we subsample balancedSubset = random.sample(currentSubset, maxSamples) - # Subsamples if it is + # Subsample if useWeights: # Adjusts to weights balancedSubset = self.subSample(balancedSubset, 1 - weights[i][j]) @@ -260,7 +259,7 @@ def balancedDataset(self): for data in linLabeled: rawDataset.append(data) - + self.dataset = rawDataset return rawDataset @@ -272,19 +271,18 @@ def balancedDataset(self): if __name__ == "__main__": - # Crear una instancia de tu conjunto de datos - dataset = rosbagDataset(main_dir=DATA_PATH, transform=dataset_transforms, boolAug=True, dataAument=2) + # Creates data instance + dataset = rosbagDataset(main_dir=DATA_PATH, transform=dataset_transforms, boolAug=True, dataAugment=2) - # Mostrar algunas imágenes y sus velocidades asociadas - for i in range(15): # Mostrar las primeras 5 imágenes + # Shows some images with the augmentation + for i in range(15): idx = random.randint(0, len(dataset) - 1) velocity, image = dataset[idx] - # Deshacer el cambio de forma y la normalización para mostrar la imagen correctamente image = image.permute(1, 2, 0).cpu().numpy() - image = (image * 0.5) + 0.5 # Desnormalizar + image = (image * 0.5) + 0.5 - # Mostrar la imagen y su velocidad asociada + # Shows the image and the associated velocity plt.imshow(image) plt.title(f"Velocidad: {velocity}") plt.show() \ No newline at end of file diff --git a/src/drone_driver/include/models.py b/src/drone_driver/include/models.py index 837ac79..5f6bf55 100755 --- a/src/drone_driver/include/models.py +++ b/src/drone_driver/include/models.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python3waas +#!/usr/bin/env python3 from torch import nn diff --git a/src/drone_driver/launch/as2_sim_circuit.launch.py b/src/drone_driver/launch/as2_sim_circuit.launch.py index 6a16184..241e3b2 100644 --- a/src/drone_driver/launch/as2_sim_circuit.launch.py +++ b/src/drone_driver/launch/as2_sim_circuit.launch.py @@ -38,7 +38,7 @@ def generate_launch_description(): name="gazebo", additional_env=env_vars, - # Closes the tmux sesion on gazebo exit + # Closes the tmux session on gazebo exit on_exit=[ OpaqueFunction( function=exit_process_function, @@ -60,10 +60,10 @@ def generate_launch_description(): ) tmuxAttach = ExecuteProcess( - # Aerostack2 depuration + # Aerostack2 terminal # cmd=['gnome-terminal', '--', 'tmux', 'attach-session', '-t', namespace], - # No aditional window + # No additional window cmd=['tmux', 'attach-session', '-t', namespace], name="attach", ) diff --git a/src/drone_driver/launch/as2_sim_ocean.launch.py b/src/drone_driver/launch/as2_sim_ocean.launch.py index 3315370..1fecff5 100644 --- a/src/drone_driver/launch/as2_sim_ocean.launch.py +++ b/src/drone_driver/launch/as2_sim_ocean.launch.py @@ -1,9 +1,7 @@ import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, ExecuteProcess, LogInfo -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration +from launch.actions import ExecuteProcess from launch_ros.actions import Node namespace= "drone0" @@ -15,20 +13,6 @@ def generate_launch_description(): worlds_dir = os.path.join(get_package_share_directory('drone_driver'), 'worlds') tmux_yml = os.path.join(get_package_share_directory('drone_driver'), 'config/tmuxLaunch.yml') - # # Default gazebo launch - - # gazebo = IncludeLaunchDescription( - # PythonLaunchDescriptionSource([os.path.join( - # get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']), - - # launch_arguments={ - # 'world': worlds + 'ocean_simulation.world', - # 'namespace': LaunchConfiguration('namespace'), - # 'use_sim_time': LaunchConfiguration('sim_time'), - # 'simulation_config_file': sim_config + '/world.json' - # }.items(), - # ) - # Px4 autopilot gazebo launch gazeboPx4 = ExecuteProcess( cmd=[ diff --git a/src/drone_driver/launch/expertPilot.launch.py b/src/drone_driver/launch/expertPilot.launch.py index 58ab012..5a5bad4 100644 --- a/src/drone_driver/launch/expertPilot.launch.py +++ b/src/drone_driver/launch/expertPilot.launch.py @@ -1,4 +1,3 @@ -import os from launch import LaunchDescription from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node diff --git a/src/drone_driver/src/droneExpertPilot.py b/src/drone_driver/src/droneExpertPilot.py index df20e2d..61d5703 100755 --- a/src/drone_driver/src/droneExpertPilot.py +++ b/src/drone_driver/src/droneExpertPilot.py @@ -8,6 +8,7 @@ from as2_python_api.modules.motion_reference_handler_module import MotionReferenceHandlerModule from sensor_msgs.msg import Image from cv_bridge import CvBridge +from geometry_msgs.msg import Point import sys import numpy as np import os @@ -26,19 +27,19 @@ # Image parameters BOTTOM_LIMIT_UMBRAL = 40 UPPER_LIMIT_UMBRAL = 10 -UPPER_PROPORTION = 0.6 -LOWER_PROPORTION = 0.4 -BREAK_INCREMENT = 0.6 +UPPER_PROPORTION = 0.4 +LOWER_PROPORTION = 0.6 +BREAK_INCREMENT = 1.2 # Vel control -MAX_ANGULAR = 3.0 -MAX_LINEAR = 6.0 -MIN_LINEAR = 2.5 +MAX_ANGULAR = 2.0 +MAX_LINEAR = 6.3 +MIN_LINEAR = 3.0 MAX_Z = 2.0 -## PID controlers -ANG_KP = 2.0 -ANG_KD = 1.75 +## PID controllers +ANG_KP = 1.5 +ANG_KD = 1.4 ANG_KI = 0.0 Z_KP = 0.5 @@ -46,9 +47,19 @@ Z_KI = 0.0 LIN_KP = 1.0 -LIN_KD = 1.20 +LIN_KD = 30.00 LIN_KI = 0.0 +LINEAL_ARRAY_LENGTH = 150 + +# Frequency's +VEL_PUBLISH_FREQ = 0.05 +SAVE_FREQ = 5 + +# Image +NUM_POINTS = 10 +SPACE = 20 + class droneController(DroneInterface): @@ -56,6 +67,7 @@ def __init__(self, drone_id: str = "drone0", verbose: bool = False, use_sim_time super().__init__(drone_id, verbose, use_sim_time) self.motion_ref_handler = MotionReferenceHandlerModule(drone=self) + self.velPublisher_ = self.create_publisher(Point, 'commanded_vels', 10) self.imageSubscription = self.create_subscription(Image, '/filtered_img', self.listener_callback, 1) # PIDs @@ -73,17 +85,11 @@ def __init__(self, drone_id: str = "drone0", verbose: bool = False, use_sim_time self.ang_rang = MAX_ANGULAR - (- MAX_ANGULAR) self.linearVel = MAX_LINEAR - # Create timers - # self.timer = self.create_timer(0.1, self.timer_callback) - #self.saver = self.create_timer(5.0, self.save_data) - - - #self.timer. - # Frequency analysis self.image_timestamps = [] self.vel_timestamps = [] self.profiling = [] + self.lastVels = [] self.cv_image = None @@ -159,7 +165,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]) @@ -175,7 +181,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]) @@ -183,37 +189,64 @@ def set_vel(self, vx, vy, vz, yaw): self.motion_ref_handler.speed.send_speed_command_with_yaw_speed( [float(velX), float(velY), float(vz)], 'earth', float(yaw)) - def get_angular_vel(self, farestPoint, nearestPoint): + def get_angular_vel(self, farthestPoint, nearestPoint): widthCenter = self.cv_image.shape[1] / 2 + topPoint = search_top_line(self.cv_image) + bottomPoint = search_bottom_line(self.cv_image) + lineDiv = NUM_POINTS + + angularError = 0 + lastAng = 0 + intervals = (bottomPoint - topPoint) / lineDiv + if int(intervals) != 0: + + angularError = 0 + for i in range(0, (bottomPoint - topPoint), int(intervals)): + angularError += widthCenter - (band_midpoint(self.cv_image, topPoint + i - SPACE, topPoint + i + 1))[0] + lastAng = angularError + angularError = angularError / lineDiv + + if intervals == 0 or abs(angularError == widthCenter): + sign = 1 + if angularError != 0: + sign = abs(lastAng) / lastAng + + angularError = widthCenter * sign + # Gets the angular error - angularError = (widthCenter - farestPoint[0])*UPPER_PROPORTION + (widthCenter - nearestPoint[0])*LOWER_PROPORTION + # angularError = (widthCenter - farthestPoint[0])*UPPER_PROPORTION + (widthCenter - nearestPoint[0])*LOWER_PROPORTION # Pixel distance to angular vel transformation - angular = (((angularError - MIN_PIXEL) * self.ang_rang) / self.px_rang) + (-MAX_ANGULAR) - anguarVel = self.angular_pid.get_pid(angular) + angular = (((angularError + widthCenter) * self.ang_rang) / self.cv_image.shape[1]) + (-MAX_ANGULAR) + angularVel = self.angular_pid.get_pid(angular) - return anguarVel + return angularVel - def get_linear_vel(self, farestPoint): + def get_linear_vel(self, farthestPoint): widthCenter = self.cv_image.shape[1] / 2 - pixelError = max(widthCenter, farestPoint) - min(widthCenter, farestPoint) - + pixelError = max(widthCenter, farthestPoint) - min(widthCenter, farthestPoint) error = np.interp(abs(pixelError), (0, widthCenter), (0, MAX_LINEAR-MIN_LINEAR)) + linearError = MAX_LINEAR - error * BREAK_INCREMENT + linearVel = self.linear_pid.get_pid(linearError) + if linearVel < MIN_LINEAR: + linearVel = MIN_LINEAR + return linearVel def follow_line(self): + vels = Point() if self.info['state'] == PlatformStatus.FLYING and self.cv_image is not None: initTime = time.time() # Gets the reference points - ## Farest point + ## Farthest point topPoint = search_top_line(self.cv_image) - farestPoint = band_midpoint(self.cv_image, topPoint, topPoint + UPPER_LIMIT_UMBRAL) + farthestPoint = band_midpoint(self.cv_image, topPoint, topPoint + UPPER_LIMIT_UMBRAL) # Nearest point bottomPoint = search_bottom_line(self.cv_image) @@ -222,14 +255,25 @@ def follow_line(self): # Distance point distancePoint = search_farthest_column(self.cv_image) - # Gets drone velocitys - angularVel = self.get_angular_vel(farestPoint, nearestPoint) + # Gets drone velocity's + angularVel = self.get_angular_vel(farthestPoint, nearestPoint) linearVel = self.get_linear_vel(distancePoint) - #self.get_logger().info("Angular vel = %f || Linear vel = %f" % (angularVel, linearVel)) + + # Smooths linear vel with circular array + self.lastVels.append(linearVel) + if len(self.lastVels) > LINEAL_ARRAY_LENGTH: + self.lastVels.pop(0) + + linearVel = np.mean(self.lastVels) + + # Publish the info for training + vels.x = float(linearVel) + vels.y = float(angularVel) + vels.z = MAX_Z + self.velPublisher_.publish(vels) # Set the velocity - # self.set_vel(self.linearVel, 0, 0, anguarVel) - # self.get_logger().info("Linear = %f | Angular = %f" % (linearVel, angularVel)) + self.get_logger().info("Linear = %f | Angular = %f" % (linearVel, angularVel)) self.set_vel2D(linearVel, 0, MAX_Z, angularVel) self.vel_timestamps.append(time.time()) @@ -255,13 +299,13 @@ def main(args=None): while rclpy.ok(): drone.follow_line() - if time.time() - initTime >= 5: + if time.time() - initTime >= SAVE_FREQ: drone.save_data() initTime = time.time() # Process a single iteration of the ROS event loop - rclpy.spin_once(drone, timeout_sec=0.05) + rclpy.spin_once(drone, timeout_sec=VEL_PUBLISH_FREQ) # End of execution drone.destroy_node() diff --git a/src/drone_driver/src/droneNeuralPilot.py b/src/drone_driver/src/droneNeuralPilot.py index 72dea8a..ba92554 100755 --- a/src/drone_driver/src/droneNeuralPilot.py +++ b/src/drone_driver/src/droneNeuralPilot.py @@ -57,9 +57,6 @@ def __init__(self, drone_id: str = "drone0", verbose: bool = False, use_sim_time # Control self.linearVel = 0 - timerPeriod = 0.01 # seconds - saveDataPeriod = 5.0 - # Frequency analysis self.image_timestamps = [] self.vel_timestamps = [] @@ -91,13 +88,12 @@ def listener_callback(self, msg): # Converts the image to cv2 format bridge = CvBridge() cv_image = bridge.imgmsg_to_cv2(msg, "bgr8") - - # Resize the image resized_image = cv2.resize(cv_image, (64, 64)) + + self.image_array = np.array(resized_image) # Convert the resized image to a tensor for inference initTime = time.time() - self.image_array = np.array(resized_image) img_tensor = dataset_transforms(self.image_array).to(self.device) self.imageTensor = img_tensor.unsqueeze(0) @@ -180,8 +176,7 @@ def get_vels(self): # Angular inference for neural network if self.imageTensor is not None: - vels = self.model(self.imageTensor)[0] - vels[1] = vels[1] / 10 + vels = self.model(self.imageTensor)[0].tolist() self.profiling.append(f"\nAngular inference = {time.time() - initTime}") return vels @@ -196,7 +191,7 @@ def follow_line(self): self.get_logger().info("Linear inference = %f | Angular inference = %f" % (vels[0], vels[1])) # Set the velocity - self.set_vel2D(vels[0], 0, MAX_Z, vels[1]) + self.set_vel2D(float(vels[0]), 0, MAX_Z, float(vels[1]/3)) # Profiling self.vel_timestamps.append(time.time()) @@ -247,4 +242,3 @@ def main(args=None): main() - diff --git a/src/drone_driver/src/image_filter_node.py b/src/drone_driver/src/image_filter_node.py index fa9195f..7a9c8d9 100755 --- a/src/drone_driver/src/image_filter_node.py +++ b/src/drone_driver/src/image_filter_node.py @@ -20,9 +20,7 @@ sys.path.append(package_path) from include.control_functions import band_midpoint, search_top_line, search_bottom_line, save_profiling, search_farthest_column -from droneExpertPilot import BOTTOM_LIMIT_UMBRAL, UPPER_LIMIT_UMBRAL - - +from droneExpertPilot import NUM_POINTS, SPACE MIN_PIXEL = -360 MAX_PIXEL = 360 @@ -39,7 +37,6 @@ TRACE_COLOR = [0, 255, 0] RADIUS = 2 - class imageFilterNode(Node): def __init__(self, out_dir=".", trace=True): @@ -67,15 +64,28 @@ def show_trace(self, label, mono_img, original): top_point = search_top_line(mono_img) bottom_point = search_bottom_line(mono_img) - red_farest = band_midpoint(mono_img, top_point, top_point + UPPER_LIMIT_UMBRAL) - red_nearest = band_midpoint(mono_img, bottom_point - BOTTOM_LIMIT_UMBRAL, bottom_point) # Image to 3 channels mono_img_color = cv2.merge([mono_img, mono_img, mono_img]) - # Dibujar los puntos en verde en la imagen original - cv2.circle(mono_img_color, (red_farest[0], top_point), 5, (0, 255, 0), -1) - cv2.circle(mono_img_color, (red_nearest[0], bottom_point), 5, (0, 255, 0), -1) + topPoint = search_top_line(mono_img) + bottomPoint = search_bottom_line(mono_img) + lineDiv = NUM_POINTS + + error = 0 + # self.get_logger().info("%d %d" % (bottomPoint, topPoint)) + intervals = (bottomPoint - topPoint) / lineDiv + if int(intervals) != 0: + for i in range(0, (bottomPoint - topPoint), int(intervals)): + error = band_midpoint(mono_img, topPoint + i - SPACE , topPoint + i + 1) + #self.get_logger().info("%d %d %d" % (error[0], error[1], i)) + + + # Draws the points in the original image + cv2.circle(mono_img_color, (int(error[0]), int(i + top_point)), 5, (0, 255, 0), -1) + + + #cv2.circle(mono_img_color, (red_nearest[0], bottom_point), 5, (0, 255, 0), -1) cv2.circle(mono_img_color, (distancePoint, top_point), 5, (255, 0, 0), -1) # Show the image cv2.imshow(label, mono_img_color) @@ -91,8 +101,8 @@ def color_filter(self, image): return red_mask def image_aperture(self, mask): - erosion_kernel = np.ones((2, 2), np.uint8) - dilate_kernel = np.ones((7, 7), np.uint8) + erosion_kernel = np.ones((1, 1), np.uint8) + dilate_kernel = np.ones((10, 10), np.uint8) n_erosion = 1 n_dilatation = 1 @@ -103,7 +113,7 @@ def image_aperture(self, mask): return dilated_mask def filter_contours(self, contours): - limit_area = 100 + limit_area = 20 # Filters small contours big_contours = [contour for contour in contours if cv2.contourArea(contour) > limit_area] @@ -119,7 +129,7 @@ def filter_contours(self, contours): def listener_callback(self, msg): - CallinitTime = time.time() + CallInitTime = time.time() bridge = CvBridge() cv_image = bridge.imgmsg_to_cv2(msg, "bgr8") @@ -133,7 +143,7 @@ def listener_callback(self, msg): self.profiling.append(f"\nFilter time = {time.time() - initTime}") - # Draws tonly the big contour + # Draws only the big contour initTime = time.time() mono_img = np.zeros_like(img) @@ -146,13 +156,13 @@ def listener_callback(self, msg): msg = bridge.cv2_to_imgmsg(mono_img, encoding="mono8") self.filteredPublisher_.publish(msg) - self.profiling.append(f"\nCallback time = {time.time() - CallinitTime}") + self.profiling.append(f"\nCallback time = {time.time() - CallInitTime}") # Traces # Display the image with contours if self.traceBool: - self.show_trace("Countors: ", mono_img, img) + self.show_trace("Outline: ", mono_img, img) def main(args=None): diff --git a/src/drone_driver/src/plotFunctions/datasetGraphic.py b/src/drone_driver/src/plotFunctions/datasetGraphic.py index a80469a..1e9438f 100644 --- a/src/drone_driver/src/plotFunctions/datasetGraphic.py +++ b/src/drone_driver/src/plotFunctions/datasetGraphic.py @@ -1,8 +1,6 @@ import matplotlib.pyplot as plt -from mpl_toolkits.mplot3d import Axes3D + import numpy as np -import ament_index_python -from imblearn.over_sampling import RandomOverSampler import sys from ament_index_python.packages import get_package_share_directory @@ -11,7 +9,7 @@ from include.data import rosbagDataset, dataset_transforms, ANGULAR_UMBRALS, LINEAR_UMBRALS -DATA_PATH = "../../training_dataset" +DATA_PATH = "../../training_dataset/expertPilot_V4" def plot_3d_bars(ax, x, y, z, xlabel='X', ylabel='Y', zlabel='Z', title='3D Plot'): @@ -90,7 +88,7 @@ def get_labels(vels): labels = [] for vel in vels: - labels.append(vel2label(vel[0], vel[1]/10)) + labels.append(vel2label(vel[0], vel[1]/3)) return labels @@ -127,21 +125,20 @@ def main(): # Gets the raw dataset data1= rosbagDataset(DATA_PATH, dataset_transforms, False, 1) - rawVels = [velocitys for image, velocitys in data1.dataset] + rawVels = [velocities for image, velocities in data1.dataset] # Gets the balanced dataset - balancedDataset = data1.balancedDataset() - balancedVels = [velocitys for image, velocitys in balancedDataset] + balancedVels = [velocities for image, velocities in data1.balancedDataset()] # Gets the augmented dataset - data2 = rosbagDataset(DATA_PATH, dataset_transforms, True, 2) - augmentedVels = [] - for i in range(len(data2)): - velocity, image = data2[i] - augmentedVels.append(velocity.cpu().detach().numpy()) + # data2 = rosbagDataset(DATA_PATH, dataset_transforms, False, 1) + # augmentedVels = [] + # for i in range(len(data2)): + # velocity, image = data2[i] + # augmentedVels.append(velocity.cpu().detach().numpy()) - graphData(["Raw datset", "Augmented dataset"], [rawVels, augmentedVels]) + graphData(["Raw dataset", "Augmented dataset"], [rawVels, balancedVels]) if __name__ == "__main__": diff --git a/src/drone_driver/src/plotFunctions/graphPath.py b/src/drone_driver/src/plotFunctions/graphPath.py index 5a426d5..b2743b2 100755 --- a/src/drone_driver/src/plotFunctions/graphPath.py +++ b/src/drone_driver/src/plotFunctions/graphPath.py @@ -23,11 +23,11 @@ def get_path(self, tf_topic): ros2_messages = ros2_reader.messages(connections=ros2_conns) for m, msg in enumerate(ros2_messages): - (connection, timestamp, rawdata) = msg + (connection, timestamp, rawData) = msg - # Searchs the tf topic + # Search the tf topic if connection.topic == tf_topic: - data = deserialize_cdr(rawdata, connection.msgtype) + data = deserialize_cdr(rawData, connection.msgtype) # Appends the data in the lists x_path.append(data.transforms[0].transform.translation.x) diff --git a/src/drone_driver/src/rosbag2generalDataset.py b/src/drone_driver/src/rosbag2generalDataset.py index d22aac9..2de5a82 100755 --- a/src/drone_driver/src/rosbag2generalDataset.py +++ b/src/drone_driver/src/rosbag2generalDataset.py @@ -17,9 +17,9 @@ def __init__(self, rosbag_path) -> None: self.firstImgTimestamp = -1 self.lastVelTimestamp = 0 - self.asocciatedImage = False + self.associatedImage = False self.lastImgTimestamp = 0 - self.asocciatedVel = True + self.associatedVel = True def transform_data(self, img_topic, vel_topic): @@ -44,36 +44,36 @@ def transform_data(self, img_topic, vel_topic): ros2_messages = ros2_reader.messages(connections=ros2_conns) - # Generates all the mesaures of the topic + # Generates all the measures of the topic for m, msg in enumerate(ros2_messages): - (connection, timestamp, rawdata) = msg + (connection, timestamp, rawData) = msg # Checks if it is the velocities topic if connection.topic == vel_topic: - data = deserialize_cdr(rawdata, connection.msgtype) - angular = data.twist.angular.z + data = deserialize_cdr(rawData, connection.msgtype) + angular = data.y # Conversion global frame to local frame - linear = data.twist.linear.x + linear = data.x # Checks the first timestamp if self.firstVelTimestamp == -1: self.firstVelTimestamp = timestamp - if timestamp >= self.lastVelTimestamp and not self.asocciatedVel: + if timestamp >= self.lastVelTimestamp and not self.associatedVel: # Save the data into a .txt output_path = os.path.join(labels_folder_path, f"{folderNum}_{m}.txt") with open(output_path, "w") as txt_file: txt_file.write(f"{linear}, {angular}\n") - self.asocciatedImage = False + self.associatedImage = False self.lastImgTimestamp = timestamp - self.asocciatedVel = True + self.associatedVel = True # Checks if it is the image topic if connection.topic == img_topic: - data = deserialize_cdr(rawdata, connection.msgtype) + data = deserialize_cdr(rawData, connection.msgtype) # Converts the image into a readable format img = np.array(data.data, dtype=data.data.dtype) @@ -83,16 +83,16 @@ def transform_data(self, img_topic, vel_topic): if self.firstImgTimestamp == -1: self.firstImgTimestamp = timestamp - if timestamp >= self.lastImgTimestamp and not self.asocciatedImage: + if timestamp >= self.lastImgTimestamp and not self.associatedImage: # Save the data into a .jpg output_path = os.path.join(img_folder_path, f"{folderNum}_{m}.jpg") resized = cv2.resize(cvImage, (160, 120)) cv2.imwrite(output_path, cv2.cvtColor(resized, cv2.COLOR_BGR2RGB)) - self.asocciatedVel = False + self.associatedVel = False self.lastVelTimestamp = timestamp - self.asocciatedImage = True + self.associatedImage = True except: print("Folder", rosbag_dir , "is not a rosbag") @@ -113,7 +113,7 @@ def main(): dataset = rosbagDataset(args.rosbags_path) img_topic = "/drone0/sensor_measurements/frontal_camera/image_raw" - vel_topic = "/drone0/self_localization/twist" + vel_topic = "/drone0/commanded_vels" dataset.transform_data(img_topic, vel_topic) diff --git a/src/drone_driver/src/train.py b/src/drone_driver/src/train.py index 026bdce..0fcc4e7 100755 --- a/src/drone_driver/src/train.py +++ b/src/drone_driver/src/train.py @@ -20,6 +20,12 @@ writer = SummaryWriter() +BATCH_SIZE = 100 +LEARNING_RATE=1e-4 +MOMENT=0.05 + +TARGET_LOSS = 0.05 +TARGET_CONECUTIVE_LOSS = 4 def should_resume(): return "--resume" in sys.argv or "-r" in sys.argv @@ -49,15 +55,15 @@ def train(checkpointPath, datasetPath, model: pilotNet, optimizer: optim.Optimiz dataset = rosbagDataset(datasetPath, dataset_transforms) dataset.balancedDataset() - train_loader = DataLoader(dataset, batch_size=10, shuffle=True) + train_loader = DataLoader(dataset, batch_size=BATCH_SIZE, shuffle=True) print("Starting training") consecutiveEpochs = 0 - targetLoss = 0.5 - limitEpochs = 1000 + targetLoss = TARGET_LOSS + epoch = 0 - for epoch in range(limitEpochs): + while consecutiveEpochs != TARGET_CONECUTIVE_LOSS: epoch_loss = 0.0 for i, data in enumerate(train_loader, 0): @@ -80,34 +86,29 @@ def train(checkpointPath, datasetPath, model: pilotNet, optimizer: optim.Optimiz # Loss graphic writer.add_scalar("Loss/train", loss, epoch) - # Saves the model - if i % 10 == 0: # print every 2000 mini-batches - # print('[%d, %5d] loss: %.3f' % - # (epoch + 1, i + 1, loss)) + # Saves the model each 20 iterations + if i % 20 == 0: save_checkpoint(checkpointPath, model, optimizer) + epoch += 1 + # Loss each epoch averageLoss = epoch_loss / len(train_loader) - print('Epoch {} - Average Loss: {:.3f}'.format(epoch + 1, averageLoss)) + print('Epoch {} - Average Loss: {:.3f}'.format(epoch, averageLoss)) - # End criteria succeded? + # End criteria succeeded? if averageLoss <= targetLoss: consecutiveEpochs += 1 else: consecutiveEpochs = 0 - # Ends the training in n consecutive epochs with creteria succeded - if consecutiveEpochs == 4: - print('Training terminated. Consecutive epochs with average loss <= 0.5.') - break - - print('Finished Training') + print('Training terminated. Consecutive epochs with average loss <= ', targetLoss) def main(filePath, datasetPath): model = pilotNet() - optimizer = optim.SGD(model.parameters(), lr=0.001, momentum=0.05) + optimizer = optim.SGD(model.parameters(), lr=LEARNING_RATE, momentum=MOMENT) # Loads if theres another model if should_resume(): diff --git a/src/drone_driver/utils/generateDataset.sh b/src/drone_driver/utils/generateDataset.sh index fcab710..a69d9b1 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=("montreal_line" "simple_circuit" "montmelo_line") +worlds=( "simple_circuit" "montmelo_line" "nurburgring_line") orientations=(0.0 3.14) tmux new-window -n drone_recorder @@ -26,18 +26,21 @@ for world in "${worlds[@]}"; do # Executes the simulator tmux send-keys -t 0 "ros2 launch drone_driver as2_sim_circuit.launch.py world:=../worlds/$world.world yaw:=$yaw" C-m sleep 5 - - tmux send-keys -t 1 "ros2 launch drone_driver expertPilot.launch.py trace:=False out_dir:=$output_directory/profiling/$world$iteration | tee temp_output.txt" C-m + + touch temp_output.txt + tmux send-keys -t 1 "ros2 launch drone_driver expertPilot.launch.py trace:=False out_dir:=$output_directory/profiling/$world$iteration | tee ./temp_output.txt" C-m + + sleep 5 # Waits untill the drone is flying - while ! grep -q "Following" temp_output.txt; do + while ! grep -q "Following" ./temp_output.txt; do sleep 1 done - rm temp_output.txt - sleep 2 + rm ./temp_output.txt + sleep 5 # Starts recording the topics - tmux send-keys -t 2 "ros2 bag record -o $output_directory/$world$iteration /drone0/sensor_measurements/frontal_camera/image_raw /drone0/self_localization/twist" C-m + tmux send-keys -t 2 "ros2 bag record -o $output_directory/$world$iteration /drone0/sensor_measurements/frontal_camera/image_raw /drone0/commanded_vels /tf" C-m sleep "$record_time" # Sends ctrl+C to end the simulation @@ -57,7 +60,6 @@ tmux kill-pane -t 1 tmux kill-window -t drone_recorder # Converts the rosbags to a general dataset -cd ../src -python3 rosbag2generalDataset.py --rosbags_path $output_directory +python3 ../src/rosbag2generalDataset.py --rosbags_path $output_directory -tmux kill-server \ No newline at end of file +# tmux kill-server \ No newline at end of file diff --git a/src/drone_driver/utils/generateNeuralPilot.sh b/src/drone_driver/utils/generateNeuralPilot.sh new file mode 100755 index 0000000..9bcaeb1 --- /dev/null +++ b/src/drone_driver/utils/generateNeuralPilot.sh @@ -0,0 +1,21 @@ +#!/bin/bash + +# Gets the arguments +output_dir=$1 +model_path_1=$2 + +if [ -z "$output_dir" ] || [ -z "$model_path_1" ]; then + echo "Usage: $0 " + exit 1 +fi + + +# Generates the dataset +./generateDataset.sh 270 $output_dir + + +# Training +python3 ../src/train.py --dataset_path $output_dir --network_path $model_path_1 #--weights True + + +./trainAndCheck.sh $output_dir $output_dir $model_path_1 false \ No newline at end of file