Merge pull request #721 from JdeRobot/issue_720

Solves Issue 720, added functionality to test models in CARLA using only the CARLA API instead of ROS/ROS2

Author: Amglega

.gitignore

@@ -101,6 +101,9 @@ share/python-wheels/
 *.egg
 MANIFEST
 
+#Bird-eye-view for CARLA
+carla-birdeye-view/*
+
 # PyInstaller
 #  Usually these files are written by a python script from a template
 #  before PyInstaller builds the exe, so as to inject date/other infos into it.
@@ -195,3 +198,4 @@ Gemfile.lock
 
 .idea
 .BehaviorMetrics
+*.onnx

behavior_metrics/brains/CARLA/brain_carla_front_image_api.py

@@ -0,0 +1,114 @@
+#!/usr/bin/python
+# -*- coding: utf-8 -*-
+import numpy as np
+import threading
+import time
+from utils.constants import DATASETS_DIR, ROOT_PATH
+import cv2 as cv
+import torch
+import torch.nn as nn
+import torchvision.models as models
+from torchvision import transforms
+from brains.CARLA.utils.pilotnet import PilotNet
+import timm
+GENERATED_DATASETS_DIR = ROOT_PATH + '/' + DATASETS_DIR
+
+
+class Brain:
+
+    def __init__(self, sensors, actuators, handler, config=None):
+        
+        # Obtain sensors
+        self.camera = sensors.get_camera('camera_0')
+        self.pose = sensors.get_pose3d('pose3d_0')
+        # Get actuators
+        self.motors = actuators.get_motor('motors_0')
+        
+        self.handler = handler
+        self.config = config
+
+        self.cont = 0
+        self.iteration = 0
+        
+        # Create model object
+        self.device = torch.device("cuda" if torch.cuda.is_available() else "cpu")
+        self.preprocess = transforms.Compose([
+            transforms.ToTensor()
+        ])
+
+        self.input_size = config['ImageSize']
+
+
+        if config['ModelName'] == 'mobilenet_large':
+            self.model = models.mobilenet_v3_large()
+            num_ftrs = self.model.classifier[-1].in_features
+            self.model.classifier[-1] = nn.Linear(num_ftrs, 2)
+        elif config['ModelName'] == 'mobilenet_small':
+            self.model = models.mobilenet_v3_small()
+            num_ftrs = self.model.classifier[-1].in_features
+            self.model.classifier[-1] = nn.Linear(num_ftrs, 2)
+        elif config['ModelName'] == 'resnet':
+            self.model = models.resnet18()
+            num_ftrs = self.model.fc.in_features
+            self.model.fc = nn.Linear(num_ftrs, 2)
+        elif config['ModelName'] == 'efficientnet_v2':
+            self.model = models.efficientnet_v2_s(weights=None)
+            num_ftrs = self.model.classifier[-1].in_features
+            self.model.classifier[-1] = torch.nn.Linear(num_ftrs, 2)
+        elif config['ModelName'] == 'efficientvit':
+            model = timm.create_model('efficientvit_b0', pretrained=False)
+            num_ftrs = model.head.classifier[-1].in_features
+            model.head.classifier[-1] = nn.Linear(num_ftrs, 2)
+        elif config['ModelName'] == 'fastvit':
+            model = timm.create_model('fastvit_mci0', pretrained=False)
+            num_ftrs = model.head.classifier[-1].in_features
+            model.head.classifier[-1] = nn.Linear(num_ftrs, 2)
+        else:
+            self.model = PilotNet(self.input_size, 2)
+        # Load the state dictionary from the local .pth file
+        state_dict = torch.load(config['SavedModelPath'], weights_only=True)
+        # Load the state dictionary into the model
+        self.model.load_state_dict(state_dict)
+
+        # Move the model to the selected device (cpu or gpu)
+        self.model.to(self.device)
+    
+        # Set the model to evaluation mode
+        self.model.eval()
+
+        time.sleep(2)
+
+    def update_frame(self, frame_id, data):
+        """Update the information to be shown in one of the GUI's frames.
+
+        Arguments:
+            frame_id {str} -- Id of the frame that will represent the data
+            data {*} -- Data to be shown in the frame. Depending on the type of frame (rgbimage, laser, pose3d, etc)
+        """
+        self.handler.update_frame(frame_id, data)
+
+    def update_pose(self, pose_data):
+        self.handler.update_pose3d(pose_data)
+
+    def execute(self):
+        image = self.camera.getImage()
+        if image is not None:
+            #image = cv.cvtColor(image, cv.COLOR_BGR2RGB)
+            cropped_image = image[240:480, 0:640]
+            resized_image = cv.resize(cropped_image, (int(self.input_size[1]), int(self.input_size[0])))
+            input_tensor = self.preprocess(resized_image).to(self.device)
+            input_batch = input_tensor.unsqueeze(0)
+            output = self.model(input_batch)
+            if self.device == "cpu":
+                net_throttle = output[0].detach().numpy()[0].item()
+                net_steer = output[0].detach().numpy()[1].item()
+            else:
+                net_throttle = output.data.cpu().numpy()[0][0].item()
+                net_steer = output.data.cpu().numpy()[0][1].item()
+            
+            self.motors.sendThrottle(net_throttle)
+            print(net_steer)
+            self.motors.sendSteer(net_steer)
+            self.update_frame('frame_0', image)
+        self.update_pose(self.pose.getPose3d())
+        #print(self.pose.getPose3d())

behavior_metrics/brains/CARLA/brain_carla_slow_and_turn_api.py

@@ -0,0 +1,65 @@
+#!/usr/bin/python
+# -*- coding: utf-8 -*-
+import numpy as np
+import threading
+import time
+from utils.constants import DATASETS_DIR, ROOT_PATH
+
+
+GENERATED_DATASETS_DIR = ROOT_PATH + '/' + DATASETS_DIR
+
+
+class Brain:
+
+    def __init__(self, sensors, actuators, handler, config=None):
+        self.camera = sensors.get_camera('camera_0')
+        self.camera_1 = sensors.get_camera('camera_1')
+        self.camera_2 = sensors.get_camera('camera_2')
+
+        self.pose = sensors.get_pose3d('pose3d_0')
+
+        self.handler = handler
+        self.config = config
+        self.motors = actuators.get_motor('motors_0')
+        self.threshold_image = np.zeros((640, 360, 3), np.uint8)
+        self.color_image = np.zeros((640, 360, 3), np.uint8)
+        self.lock = threading.Lock()
+        self.threshold_image_lock = threading.Lock()
+        self.color_image_lock = threading.Lock()
+        self.cont = 0
+        self.iteration = 0
+
+        # self.previous_timestamp = 0
+        # self.previous_image = 0
+
+        self.previous_v = None
+        self.previous_w = None
+        self.previous_w_normalized = None
+
+        time.sleep(2)
+
+    def update_frame(self, frame_id, data):
+        """Update the information to be shown in one of the GUI's frames.
+
+        Arguments:
+            frame_id {str} -- Id of the frame that will represent the data
+            data {*} -- Data to be shown in the frame. Depending on the type of frame (rgbimage, laser, pose3d, etc)
+        """
+        self.handler.update_frame(frame_id, data)
+
+    def update_pose(self, pose_data):
+        self.handler.update_pose3d(pose_data)
+
+    def execute(self):
+        image = self.camera.getImage()
+        image_1 = self.camera_1.getImage()
+        image_2 = self.camera_2.getImage()
+
+        self.motors.sendThrottle(0.5)
+        self.motors.sendSteer(0.05)
+
+        self.update_frame('frame_0', image)
+        self.update_frame('frame_1', image_1)
+        self.update_frame('frame_2', image_2)
+        self.update_pose(self.pose.getPose3d())
+        #print(self.pose.getPose3d())

behavior_metrics/brains/CARLA/brain_modifiedDeepestLSTM.py

@@ -82,19 +82,6 @@ def __init__(self, sensors, actuators, model=None, handler=None, config=None):
             monolithic_model_path = PRETRAINED_MODELS + model
             print("Loading model from: ", monolithic_model_path)
 
-            # self.monolithic_model = ModifiedDeepestLSTM(image_shape=(66, 200, 3), num_labels=2)
-            # self.monolithic_model = resnet18(weights=ResNet18_Weights.DEFAULT)
-            # self.monolithic_model.fc = nn.Linear(self.monolithic_model.fc.in_features, 2)
-            # self.monolithic_model.load_state_dict(torch.load(monolithic_model_path, map_location=self.device))
-            # self.monolithic_model.to(self.device)
-            # self.monolithic_model.eval()
-            
-            # self.monolithic_model = efficientnet_v2_s(weights=None)
-            # self.monolithic_model.classifier[-1] = nn.Linear(self.monolithic_model.classifier[-1].in_features, 2)
-            # self.monolithic_model.load_state_dict(torch.load(monolithic_model_path, map_location=self.device))
-            # self.monolithic_model.to(self.device)            
-            # self.monolithic_model.eval()
-            
             # onnx model
             providers = [('CUDAExecutionProvider', {})] if ort.get_available_providers().__contains__('CUDAExecutionProvider') else ['CPUExecutionProvider']
             self.ort_session = ort.InferenceSession(str(monolithic_model_path), providers=providers)
@@ -116,7 +103,7 @@ def __init__(self, sensors, actuators, model=None, handler=None, config=None):
         self.prev_yaw = None
         self.delta_yaw = 0
 
-        self.target_point = [160.0,-105.3,0.42,0.00,0.00,180.00]
+        self.target_point = [160.0,108.3,0.42,0.00,0.00,180.00]
         self.termination_code = 0  # 0: not terminated; 1: arrived at target; 2: wrong turn
 
         self._last_tick = None # para calcular el tiempo entre ticks
@@ -158,8 +145,15 @@ def update_frame(self, frame_id, data):
 
     def predict_controls(self, image_seg): # se saco model como argumento
         # Asegurarse de que sea una imagen BGR como la cargada por cv2.imread
-        calzada_color = [128, 64, 128]
-        mask = cv2.inRange(image_seg, np.array(calzada_color), np.array(calzada_color))
+        if image_seg is None:
+            raise ValueError("Segmented image is None (sensor may not be ready).")
+
+        if not isinstance(image_seg, np.ndarray):
+            print(f"[DEBUG] Segmentation image type: {type(image_seg)}")
+            
+            
+        calzada_color =np.array([128, 64, 128], dtype=np.uint8)
+        mask = cv2.inRange(image_seg, calzada_color, calzada_color)
 
         masked_image = np.zeros_like(image_seg)
         masked_image[mask > 0] = [255, 255, 255]
@@ -176,16 +170,10 @@ def predict_controls(self, image_seg): # se saco model como argumento
 
         gray = cv2.cvtColor(resized, cv2.COLOR_BGR2GRAY)
         rgb_like = cv2.merge([gray, gray, gray])
-
+        
+        # rgb_like /= 255.0
+        
         input_tensor = torch.tensor(rgb_like, dtype=torch.float32).permute(2, 0, 1) 
-        # input_tensor = input_tensor.unsqueeze(0).to(self.device)
-
-        # with torch.no_grad():
-        #     prediction = model(input_tensor)
-
-        # # steer, throttle = prediction[0].tolist()
-        # steer = prediction[0][0].item()
-        # throttle = prediction[0][1].item()
 
         input_np = input_tensor.unsqueeze(0).cpu().numpy()        # [1,3,66,200] float32
 
@@ -206,22 +194,12 @@ def predict_controls(self, image_seg): # se saco model como argumento
 
     def execute(self):
         """Main loop of the brain. This will be called iteratively each TIME_CYCLE (see pilot.py)"""      
-        rgb_image = self.camera_rgb.getImage().data  # rgb_image shape:  (768, 1024, 3)     
-        seg_image = self.camera_seg.getImage().data   # seg_image shape:  (80, 400, 3)   
-             
+        rgb_image = self.camera_rgb.getImage()  #.data  # rgb_image shape:  (768, 1024, 3)     
+        seg_image = self.camera_seg.getImage()  #.data   # seg_image shape:  (80, 400, 3)   
+        
         self.update_frame("frame_0", rgb_image)
         self.update_frame("frame_1", seg_image)
-    
-        # if hasattr(self, 'pilot'):
-        #     current = self.pilot.tick_counter
-        #     if self._last_tick is None:
-        #         delta = 0
-        #     else:
-        #         delta = current - self._last_tick
-        #     logger.info(f"{delta} ticks -> inferencia")
-        #     self._last_tick = current
-        
-        # print("Inferencia ejecutandose") # (600, 800, 3)
+
 
         steer, throttle = self.predict_controls(seg_image) # se saco self.monolithic_model como argumento       
         # print(f"Predicted - Steer: {steer:.3f}, Throttle: {throttle:.3f}")
@@ -236,18 +214,29 @@ def execute(self):
         #         f"target: ({self.target_point[0]:.2f}, {self.target_point[1]:.2f})")
 
         if self.target_point is not None:
-            distance_to_target = np.sqrt(
-                (self.target_point.x - vehicle_location.x) ** 2 +
-                (self.target_point.y - (-vehicle_location.y)) ** 2)
-            
-            # print(f'Euclidean distance to target: {distance_to_target}')
+            # Aceptar lista o Location
+            if isinstance(self.target_point, (list, tuple, np.ndarray)):
+                tx, ty = self.target_point[0], self.target_point[1]
+            elif hasattr(self.target_point, "x"):
+                tx, ty = self.target_point.x, self.target_point.y
+            else:
+                raise ValueError(f"Unsupported type for target_point: {type(self.target_point)}")
+
+            # Coordenadas actuales
+            vx, vy = vehicle_location.x, vehicle_location.y
+            # print(f"x: {vx} -> {tx}, y: {vy} -> {ty}")
+
+            # Calcular distancia euclidiana 2D
+            distance_to_target = math.hypot(tx - vx, ty - vy)
+
             if distance_to_target < 1.5:
                 self.termination_code = 1
-                arrived = True
-                print(f"======== Arrived at target point {distance_to_target} m away============.")
-                
+                print(f"======== Arrived at target point ({tx:.1f}, {ty:.1f}) | {distance_to_target:.2f} m away ==========")
+                        
 
         self.motors.sendThrottle(throttle)
         self.motors.sendSteer(steer)
         self.motors.sendBrake(0.0)
-        
+        
+       
+

behavior_metrics/brains/CARLA/utils/pilotnet.py

@@ -0,0 +1,67 @@
+import torch
+import torch.nn as nn
+
+
+class PilotNet(nn.Module):
+    def __init__(self,
+                image_shape,
+                num_labels):
+        super().__init__()
+
+        self.img_height = image_shape[0]
+        self.img_width = image_shape[1]
+        self.num_channels = 3
+
+        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.relu1 = nn.ReLU()
+        self.cn_2 = nn.Conv2d(24, 36, kernel_size=5, stride=2)
+        self.relu2 = nn.ReLU()
+        self.cn_3 = nn.Conv2d(36, 48, kernel_size=5, stride=2)
+        self.relu3 = nn.ReLU()
+        self.cn_4 = nn.Conv2d(48, 64, kernel_size=3, stride=1)
+        self.relu4 = nn.ReLU()
+        self.cn_5 = nn.Conv2d(64, 64, kernel_size=3, stride=1)
+        self.relu5 = nn.ReLU()
+        self.flatten = nn.Flatten()
+        self.fc_1 = nn.Linear(1 * 18 * 64, 1164)
+        self.relu_fc1 = nn.ReLU()
+        self.fc_2 = nn.Linear(1164, 100)
+        self.relu_fc2 = nn.ReLU()
+        self.fc_3 = nn.Linear(100, 50)
+        self.relu_fc3 = nn.ReLU()
+        self.fc_4 = nn.Linear(50, 10)
+        self.relu_fc4 = nn.ReLU()
+        self.fc_5 = nn.Linear(10, self.output_size)
+
+    def forward(self, img):
+
+        out = self.ln_1(img)
+
+        out = self.cn_1(out)
+        out = self.relu1(out)
+        out = self.cn_2(out)
+        out = self.relu2(out)
+        out = self.cn_3(out)
+        out = self.relu3(out)
+        out = self.cn_4(out)
+        out = self.relu4(out)
+        out = self.cn_5(out)
+        out = self.relu5(out)
+
+        out = self.flatten(out)
+
+        out = self.fc_1(out)
+        out = self.relu_fc1(out)
+        out = self.fc_2(out)
+        out = self.relu_fc2(out)
+        out = self.fc_3(out)
+        out = self.relu_fc3(out)
+        out = self.fc_4(out)
+        out = self.relu_fc4(out)
+        out = self.fc_5(out)
+
+        return out