From 4fe663bb30e0e7d5718dff08681ea1ee03a677ab Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Mon, 15 Apr 2024 14:19:46 +0200 Subject: [PATCH 1/3] Updated Brain V torch ATLAS --- ...in_carla_bird_eye_deep_learning_torch_V.py | 78 ++++++++++++++++-- .../brains/CARLA/pytorch/utils/pilotnet.py | 79 +++++++++++-------- .../configs/CARLA/default_carla_torch.yml | 6 +- 3 files changed, 118 insertions(+), 45 deletions(-) diff --git a/behavior_metrics/brains/CARLA/pytorch/brain_carla_bird_eye_deep_learning_torch_V.py b/behavior_metrics/brains/CARLA/pytorch/brain_carla_bird_eye_deep_learning_torch_V.py index 9b545d96..cfe77308 100644 --- a/behavior_metrics/brains/CARLA/pytorch/brain_carla_bird_eye_deep_learning_torch_V.py +++ b/behavior_metrics/brains/CARLA/pytorch/brain_carla_bird_eye_deep_learning_torch_V.py @@ -68,7 +68,9 @@ def __init__(self, sensors, actuators, model=None, handler=None, config=None): self.net = torch.jit.load(PRETRAINED_MODELS + model).to(self.device) # self.clean_model() else: - self.net = PilotNet((200,66,4), 3).to(self.device) + #self.net = PilotNet((200,66,4), 3).to(self.device) + #self.net = PilotNet((200,66,4), 2).to(self.device) + self.net = PilotNet((66,200,4), 2).to(self.device) self.net.load_state_dict(torch.load(PRETRAINED_MODELS + model,map_location=self.device)) else: print("Brain not loaded") @@ -119,27 +121,89 @@ def execute(self): self.update_frame('frame_2', image) self.update_frame('frame_3', image_3) - self.update_frame('frame_0', bird_eye_view_1) + #self.update_frame('frame_0', bird_eye_view_1) + self.update_frame('frame_0', np.array(image)) try: image = Image.fromarray(image) + + #print(image.size) + #image = image.resize((300, 225)) + #image = image.resize((225, 300)) + #print(image.size) + #image.save('imagen_guardada.png') + # (2048, 1536) + # (2048, 120) + + + # ATLAS original + #camera_bp.set_attribute('image_size_x', str(2048)) + #camera_bp.set_attribute('image_size_y', str(1536)) + # We adjust the size to retrieve images at a faster pace + #camera_bp.set_attribute('image_size_x', str(300)) + #camera_bp.set_attribute('image_size_y', str(224.85)) + + altura_recorte = 120 + ancho, altura = image.size + y_superior = max(0, altura - altura_recorte) + image = image.crop((0, y_superior, ancho, altura)) + + #print('image.size', image.size) + + + + image = self.transformations(image) + + image = image / 255.0 speed = self.vehicle.get_velocity() vehicle_speed = 3.6 * math.sqrt(speed.x**2 + speed.y**2 + speed.z**2) - valor_cuartadimension = torch.full((1, image.shape[1], image.shape[2]), float(vehicle_speed)) + #print('vehicle_speed', vehicle_speed) + + vehicle_speed_norm = torch.clamp(torch.tensor(vehicle_speed, dtype=torch.float32) / 40.0, 0, 1.0) + + + valor_cuartadimension = torch.full((1, image.shape[1], image.shape[2]), float(vehicle_speed_norm)) image = torch.cat((image, valor_cuartadimension), dim=0).to(self.device) image = image.unsqueeze(0) + + #print('image.shape', image.shape) + #print('self.gpu_inference', self.gpu_inference) + #print(image) start_time = time.time() with torch.no_grad(): prediction = self.net(image).cpu().numpy() if self.gpu_inference else self.net(image).numpy() self.inference_times.append(time.time() - start_time) - throttle = prediction[0][0] - steer = prediction[0][1] * (1 - (-1)) + (-1) - break_command = prediction[0][2] + print('prediction', prediction) + if (prediction[0][1] < 0.4): + print('LOG!!!!!!!!!!!!!!!!!!!!!!!!!!!') + #print(prediction) + prediction = prediction.flatten() + # prediction = prediction.detach().cpu().numpy().flatten() + #print('prediction', prediction) + + #print('vehicle_speed', vehicle_speed) + #print('vehicle_speed_norm', vehicle_speed_norm) + + combined, steer = prediction + combined = float(combined) + throttle, break_command = 0.0, 0.0 + if combined >= 0.5: + throttle = (combined - 0.5) / 0.5 + else: + break_command = (0.5 - combined) / 0.5 + steer = (float(steer) * 2.0) - 1.0 + + #throttle = prediction[0][0] + #steer = prediction[0][1] * (1 - (-1)) + (-1) + #break_command = prediction[0][2] + + print(throttle, steer, break_command) + #print('----') if vehicle_speed > 30: self.motors.sendThrottle(0) @@ -151,7 +215,7 @@ def execute(self): self.motors.sendSteer(0.0) self.motors.sendBrake(0) else: - self.motors.sendThrottle(throttle) + self.motors.sendThrottle(0.5) self.motors.sendSteer(steer) self.motors.sendBrake(break_command) diff --git a/behavior_metrics/brains/CARLA/pytorch/utils/pilotnet.py b/behavior_metrics/brains/CARLA/pytorch/utils/pilotnet.py index 153928ae..9d65ea02 100644 --- a/behavior_metrics/brains/CARLA/pytorch/utils/pilotnet.py +++ b/behavior_metrics/brains/CARLA/pytorch/utils/pilotnet.py @@ -3,56 +3,65 @@ class PilotNet(nn.Module): - def __init__(self, - image_shape, - num_labels): + def __init__(self, image_shape, num_labels): super(PilotNet, self).__init__() - - self.img_height = image_shape[0] - self.img_width = image_shape[1] + self.num_channels = image_shape[2] + # Batch normalization? + self.batchnorm_input = nn.BatchNorm2d(self.num_channels) # Para imágenes en formato RGB (3 canales) + self.cn_1 = nn.Conv2d(in_channels=self.num_channels, out_channels=24, kernel_size=5, stride=2) + self.relu_1 = nn.ReLU() + self.cn_2 = nn.Conv2d(in_channels=24, out_channels=36, kernel_size=5, stride=2) + self.relu_2 = nn.ReLU() + self.cn_3 = nn.Conv2d(in_channels=36, out_channels=48, kernel_size=5, stride=2) + self.relu_3 = nn.ReLU() + self.cn_4 = nn.Conv2d(in_channels=48, out_channels=64, kernel_size=3, stride=1) + self.relu_4 = nn.ReLU() + self.cn_5 = nn.Conv2d(in_channels=64, out_channels=64, kernel_size=3, stride=1) + self.relu_5 = nn.ReLU() + self.dropout_1 = nn.Dropout(0.2) + self.flatten = nn.Flatten() - self.output_size = num_labels - - self.ln_1 = nn.BatchNorm2d(self.num_channels, eps=1e-03) - - self.cn_1 = nn.Conv2d(self.num_channels, 24, kernel_size=5, stride=2) - self.cn_2 = nn.Conv2d(24, 36, kernel_size=5, stride=2) - self.cn_3 = nn.Conv2d(36, 48, kernel_size=5, stride=2) - self.cn_4 = nn.Conv2d(48, 64, kernel_size=3, stride=1) - self.cn_5 = nn.Conv2d(64, 64, kernel_size=3, stride=1) - - self.fc_1 = nn.Linear(1 * 18 * 64, 1164) + # Flatten layer? + self.fc_1 = nn.Linear(1152, 1164) # add embedding layer output size + self.relu_fc_1 = nn.ReLU() self.fc_2 = nn.Linear(1164, 100) + self.relu_fc_2 = nn.ReLU() self.fc_3 = nn.Linear(100, 50) + self.relu_fc_3 = nn.ReLU() self.fc_4 = nn.Linear(50, 10) - self.fc_5 = nn.Linear(10, self.output_size) + self.relu_fc_4 = nn.ReLU() + self.fc_5 = nn.Linear(10, num_labels) def forward(self, img): - - out = self.ln_1(img) - - out = self.cn_1(out) - out = torch.relu(out) + out = self.batchnorm_input(img) + out = self.cn_1(img) + out = self.relu_1(out) out = self.cn_2(out) - out = torch.relu(out) + out = self.relu_2(out) out = self.cn_3(out) - out = torch.relu(out) + out = self.relu_3(out) + out = self.cn_4(out) - out = torch.relu(out) + out = self.relu_4(out) out = self.cn_5(out) - out = torch.relu(out) - - out = out.reshape(out.size(0), -1) - + out = self.relu_5(out) + + out = self.dropout_1(out) + + #out = out.view(-1, 1152) + out = self.flatten(out) + out = self.fc_1(out) - out = torch.relu(out) + out = self.relu_fc_1(out) out = self.fc_2(out) - out = torch.relu(out) + out = self.relu_fc_2(out) out = self.fc_3(out) - out = torch.relu(out) + out = self.relu_fc_3(out) out = self.fc_4(out) - out = torch.relu(out) + out = self.relu_fc_4(out) out = self.fc_5(out) - return out + #out = torch.sigmoid(out) + + return out \ No newline at end of file diff --git a/behavior_metrics/configs/CARLA/default_carla_torch.yml b/behavior_metrics/configs/CARLA/default_carla_torch.yml index 1cc1148e..ea6a9eee 100644 --- a/behavior_metrics/configs/CARLA/default_carla_torch.yml +++ b/behavior_metrics/configs/CARLA/default_carla_torch.yml @@ -33,11 +33,11 @@ Behaviors: Topic: '/carla/ego_vehicle/vehicle_control_cmd' MaxV: 3 MaxW: 0.3 - BrainPath: 'brains/CARLA/brain_carla_segmentation_based_imitation_learning.py' + BrainPath: 'brains/CARLA/pytorch/brain_carla_bird_eye_deep_learning_torch_V.py' PilotTimeCycle: 50 AsyncMode: True Parameters: - Model: 'pilotnet_v8.0.pth' + Model: '15_04_best_model_checkpoint_189.pth' ImageCropped: True ImageSize: [ 100,50 ] ImageNormalized: True @@ -47,7 +47,7 @@ Behaviors: ImageTranform: '' Type: 'CARLA' Simulation: - World: configs/CARLA/CARLA_launch_files/town_02_anticlockwise_imitation_learning.launch + World: configs/CARLA/CARLA_launch_files/carla_new_ada_bridge_updated.launch RandomSpawnPoint: False Dataset: In: '/tmp/my_bag.bag' From 5afe1ffb2227c4c3685690d367e29a7328b10e53 Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Mon, 29 Apr 2024 15:59:02 +0200 Subject: [PATCH 2/3] Updated brain --- ...in_carla_bird_eye_deep_learning_torch_V.py | 45 ++----------------- .../configs/CARLA/default_carla_torch.yml | 4 +- 2 files changed, 5 insertions(+), 44 deletions(-) diff --git a/behavior_metrics/brains/CARLA/pytorch/brain_carla_bird_eye_deep_learning_torch_V.py b/behavior_metrics/brains/CARLA/pytorch/brain_carla_bird_eye_deep_learning_torch_V.py index cfe77308..0ad0da05 100644 --- a/behavior_metrics/brains/CARLA/pytorch/brain_carla_bird_eye_deep_learning_torch_V.py +++ b/behavior_metrics/brains/CARLA/pytorch/brain_carla_bird_eye_deep_learning_torch_V.py @@ -127,32 +127,10 @@ def execute(self): try: image = Image.fromarray(image) - #print(image.size) - #image = image.resize((300, 225)) - #image = image.resize((225, 300)) - #print(image.size) - #image.save('imagen_guardada.png') - # (2048, 1536) - # (2048, 120) - - - # ATLAS original - #camera_bp.set_attribute('image_size_x', str(2048)) - #camera_bp.set_attribute('image_size_y', str(1536)) - # We adjust the size to retrieve images at a faster pace - #camera_bp.set_attribute('image_size_x', str(300)) - #camera_bp.set_attribute('image_size_y', str(224.85)) - altura_recorte = 120 ancho, altura = image.size y_superior = max(0, altura - altura_recorte) image = image.crop((0, y_superior, ancho, altura)) - - #print('image.size', image.size) - - - - image = self.transformations(image) @@ -161,7 +139,6 @@ def execute(self): vehicle_speed = 3.6 * math.sqrt(speed.x**2 + speed.y**2 + speed.z**2) #print('vehicle_speed', vehicle_speed) - vehicle_speed_norm = torch.clamp(torch.tensor(vehicle_speed, dtype=torch.float32) / 40.0, 0, 1.0) @@ -169,25 +146,14 @@ def execute(self): image = torch.cat((image, valor_cuartadimension), dim=0).to(self.device) image = image.unsqueeze(0) - #print('image.shape', image.shape) - #print('self.gpu_inference', self.gpu_inference) - #print(image) - start_time = time.time() with torch.no_grad(): prediction = self.net(image).cpu().numpy() if self.gpu_inference else self.net(image).numpy() self.inference_times.append(time.time() - start_time) - print('prediction', prediction) - if (prediction[0][1] < 0.4): - print('LOG!!!!!!!!!!!!!!!!!!!!!!!!!!!') + #print('prediction', prediction) #print(prediction) prediction = prediction.flatten() - # prediction = prediction.detach().cpu().numpy().flatten() - #print('prediction', prediction) - - #print('vehicle_speed', vehicle_speed) - #print('vehicle_speed_norm', vehicle_speed_norm) combined, steer = prediction combined = float(combined) @@ -197,13 +163,8 @@ def execute(self): else: break_command = (0.5 - combined) / 0.5 steer = (float(steer) * 2.0) - 1.0 + #print(throttle, steer, break_command) - #throttle = prediction[0][0] - #steer = prediction[0][1] * (1 - (-1)) + (-1) - #break_command = prediction[0][2] - - print(throttle, steer, break_command) - #print('----') if vehicle_speed > 30: self.motors.sendThrottle(0) @@ -215,7 +176,7 @@ def execute(self): self.motors.sendSteer(0.0) self.motors.sendBrake(0) else: - self.motors.sendThrottle(0.5) + self.motors.sendThrottle(throttle) self.motors.sendSteer(steer) self.motors.sendBrake(break_command) diff --git a/behavior_metrics/configs/CARLA/default_carla_torch.yml b/behavior_metrics/configs/CARLA/default_carla_torch.yml index ea6a9eee..5d140829 100644 --- a/behavior_metrics/configs/CARLA/default_carla_torch.yml +++ b/behavior_metrics/configs/CARLA/default_carla_torch.yml @@ -34,10 +34,10 @@ Behaviors: MaxV: 3 MaxW: 0.3 BrainPath: 'brains/CARLA/pytorch/brain_carla_bird_eye_deep_learning_torch_V.py' - PilotTimeCycle: 50 + PilotTimeCycle: 100 AsyncMode: True Parameters: - Model: '15_04_best_model_checkpoint_189.pth' + Model: '29_04_best_model_checkpoint_299.pth' ImageCropped: True ImageSize: [ 100,50 ] ImageNormalized: True From e225b542cfa1cfd76c26e462c84c529a7fe28db5 Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Wed, 10 Jul 2024 16:47:32 +0200 Subject: [PATCH 3/3] Updated config --- behavior_metrics/configs/CARLA/default_carla_torch.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/behavior_metrics/configs/CARLA/default_carla_torch.yml b/behavior_metrics/configs/CARLA/default_carla_torch.yml index 5d140829..53dab425 100644 --- a/behavior_metrics/configs/CARLA/default_carla_torch.yml +++ b/behavior_metrics/configs/CARLA/default_carla_torch.yml @@ -37,7 +37,7 @@ Behaviors: PilotTimeCycle: 100 AsyncMode: True Parameters: - Model: '29_04_best_model_checkpoint_299.pth' + Model: 'ATLAS_06_06_best_model_checkpoint_245.pth' ImageCropped: True ImageSize: [ 100,50 ] ImageNormalized: True @@ -47,7 +47,7 @@ Behaviors: ImageTranform: '' Type: 'CARLA' Simulation: - World: configs/CARLA/CARLA_launch_files/carla_new_ada_bridge_updated.launch + World: configs/CARLA/CARLA_launch_files/carla_new_ada_bridge_updated_parque01.launch RandomSpawnPoint: False Dataset: In: '/tmp/my_bag.bag'