Small refactor

carla_real_traffic_scenarios/ngsim/cords_mapping.py

@@ -1,13 +1,14 @@
-import logging
-
 import cv2.cv2 as cv2
 import numpy as np
 
-from carla_real_traffic_scenarios.ngsim.ngsim_recording import PIXELS_TO_METERS
 from carla_real_traffic_scenarios.ngsim import NGSimDatasets
 from carla_real_traffic_scenarios.utils.transforms import Vector2, Transform
 
-LOGGER = logging.getLogger(__name__)
+LANE_WIDTH_METERS = 3.7
+LANE_WIDTH_PIXELS = 24  # pixels / 3.7 m, lane width
+METER_TO_PIXELS = LANE_WIDTH_PIXELS / LANE_WIDTH_METERS
+PIXELS_TO_METERS = 1. / METER_TO_PIXELS
+FOOT_TO_METERS = 0.3048  # metres per foot
 
 
 class NGSimToCarlaMapper:

carla_real_traffic_scenarios/ngsim/ngsim_lanechange_scenario.py

@@ -7,14 +7,12 @@
 
 from carla_real_traffic_scenarios import DT
 from carla_real_traffic_scenarios.ngsim import FRAMES_BEFORE_MANUVEUR, FRAMES_AFTER_MANUVEUR, NGSimDataset, DatasetMode
-from carla_real_traffic_scenarios.ngsim.cords_mapping import MAPPER_BY_NGSIM_DATASET
-from carla_real_traffic_scenarios.ngsim.ngsim_carla_sync import NGSimVehiclesInCarla
 from carla_real_traffic_scenarios.ngsim.ngsim_recording import NGSimRecording, LaneChangeInstant, PIXELS_TO_METERS
 from carla_real_traffic_scenarios.scenario import ScenarioStepResult, Scenario, ChauffeurCommand
+from carla_real_traffic_scenarios.utils.carla import RealTrafficVehiclesInCarla
 from carla_real_traffic_scenarios.utils.collections import find_first_matching
 from carla_real_traffic_scenarios.utils.geometry import normalize_angle
 from carla_real_traffic_scenarios.utils.transforms import distance_between_on_plane
-from carla_real_traffic_scenarios.vehicles import VEHICLE_BY_TYPE_ID
 
 CROSSTRACK_ERROR_TOLERANCE = 0.3
 YAW_DEG_ERRORS_TOLERANCE = 10
@@ -81,11 +79,12 @@ def reset(self, vehicle: carla.Vehicle):
 
         def on_collided(e):
             self._collided = True
+
         self._collision_sensor.listen(on_collided)
 
         self._lane_change: LaneChangeInstant = random.choice(self._lane_change_instants)
 
-        self._ngsim_vehicles_in_carla = NGSimVehiclesInCarla(self._client, self._world, self._ngsim_dataset)
+        self._ngsim_vehicles_in_carla = RealTrafficVehiclesInCarla(self._client, self._world)
         self._target_alignment_counter = 0
 
         self._previous_chauffeur_command = self._lane_change.chauffeur_command
@@ -98,11 +97,9 @@ def on_collided(e):
         agent_ngsim_vehicle = find_first_matching(ngsim_vehicles, lambda v: v.id == self._lane_change.vehicle_id)
         other_ngsim_vehicles = [v for v in ngsim_vehicles if v.id != self._lane_change.vehicle_id]
 
-        mapper = MAPPER_BY_NGSIM_DATASET[self._ngsim_dataset]
-        v_data = VEHICLE_BY_TYPE_ID[vehicle.type_id]
-        t = mapper.ngsim_to_carla(agent_ngsim_vehicle.get_transform(), v_data.z_offset, v_data.rear_axle_offset)
+        t = agent_ngsim_vehicle.transform
         vehicle.set_transform(t.as_carla_transform())
-        v = t.orientation * agent_ngsim_vehicle._speed * PIXELS_TO_METERS
+        v = t.orientation * agent_ngsim_vehicle.speed * PIXELS_TO_METERS
         vehicle.set_velocity(v.to_vector3(0).as_carla_vector3d())  # meters per second,
 
         self._ngsim_vehicles_in_carla.step(other_ngsim_vehicles)

carla_real_traffic_scenarios/ngsim/ngsim_recording.py

@@ -8,18 +8,15 @@
 
 from carla_real_traffic_scenarios import DT
 from carla_real_traffic_scenarios.ngsim import FRAMES_BEFORE_MANUVEUR, FRAMES_AFTER_MANUVEUR, NGSimDataset, \
-    NGSimTimeslot, \
-    NGSimDatasets
+    NGSimTimeslot, NGSimDatasets
+from carla_real_traffic_scenarios.ngsim.cords_mapping import MAPPER_BY_NGSIM_DATASET, PIXELS_TO_METERS, \
+    LANE_WIDTH_PIXELS, METER_TO_PIXELS, FOOT_TO_METERS
 from carla_real_traffic_scenarios.scenario import ChauffeurCommand
+from carla_real_traffic_scenarios.utils.carla import RealTrafficVehicle, find_best_matching_model
 from carla_real_traffic_scenarios.utils.pandas import swap_columns_inplace
 from carla_real_traffic_scenarios.utils.transforms import Transform, Vector2
-from carla_real_traffic_scenarios.utils.units import KMH_TO_MPS
+from carla_real_traffic_scenarios.vehicles import VEHICLE_BY_TYPE_ID
 
-LANE_WIDTH_METERS = 3.7
-LANE_WIDTH_PIXELS = 24  # pixels / 3.7 m, lane width
-METER_TO_PIXELS = LANE_WIDTH_PIXELS / LANE_WIDTH_METERS
-PIXELS_TO_METERS = 1. / METER_TO_PIXELS
-FOOT_TO_METERS = 0.3048  # metres per foot
 X_OFFSET_PIXELS = 470  # horizontal offset (camera 2 leftmost view)
 MAX_SPEED = 130
 
@@ -37,7 +34,6 @@
 
 LOGGER = logging.getLogger(__name__)
 
-
 # Car coordinate system, origin under the centre of the rear axis
 #
 #      ^ y                       (x, y, x., y.)
@@ -64,21 +60,17 @@
 
 class Simulator:
 
-    def __init__(self, fps=30):
+    def __init__(self):
         self.offset = int(1.5 * LANE_WIDTH_PIXELS)
-        self.fps = fps  # updates per second
         self.frame = 0  # frame index
         self.env_cars = None  # vehicles list
-        self.mean_fps = None
-        self.look_ahead = MAX_SPEED * KMH_TO_MPS * METER_TO_PIXELS
-        self.user_is_done = None
 
 
 class NGSimCar:
     max_a = 40
     max_b = 0.01
 
-    def __init__(self, df, y_offset, kernel=0):
+    def __init__(self, df, y_offset, *, mapper, kernel=0):
         k = kernel  # running window size
         self.length_m = df.at[df.index[0], 'Vehicle Length'] * FOOT_TO_METERS
         self.width_m = df.at[df.index[0], 'Vehicle Width'] * FOOT_TO_METERS
@@ -99,6 +91,9 @@ def __init__(self, df, y_offset, kernel=0):
         self._direction = self._get('init_direction', 0)
         self._speed = self._get('speed', 0)
         self.off_screen = self._max_t <= 0
+        model = find_best_matching_model(self.width_m, self.length_m)
+        self.type_id = model.type_id
+        self._mapper = mapper
 
     def step(self, action):  # takes also the parameter action = state temporal derivative
         """
@@ -134,6 +129,11 @@ def get_transform(self) -> Transform:
             Vector2.from_numpy(self._direction),
         )
 
+    def get_carla_transform(self):
+        transform = self.get_transform()
+        model = VEHICLE_BY_TYPE_ID[self.type_id]
+        return self._mapper.ngsim_to_carla(transform, model.z_offset, model.rear_axle_offset)
+
     def get_velocity(self) -> Vector2:
         direction = Vector2.from_numpy(self._direction)
         return direction * self._speed
@@ -181,6 +181,10 @@ def action_clipping(self, a, b):
         b = b if abs(b) < max_b else np.sign(b) * max_b
         return a, b
 
+    def as_real_traffic_car(self):
+        carla_transform = self.get_carla_transform()
+        return RealTrafficVehicle(self.id, self.type_id, self.width_m, self.length_m, carla_transform, self._speed)
+
 
 class LaneChangeInstant(NamedTuple):
     timeslot: NGSimTimeslot
@@ -226,6 +230,8 @@ def __init__(self, data_dir: str, ngsim_dataset: NGSimDataset):
         self.smoothing_window = 15
         self.max_frame = -1
 
+        self._mapper = MAPPER_BY_NGSIM_DATASET[ngsim_dataset]
+
     def _init_df(self, data_dir, x_offset_meters):
         self.lane_change_instants = []
 
@@ -292,14 +298,11 @@ def reset(self, timeslot: NGSimTimeslot, frame: int):
         self.frame = frame
         self._timeslot = timeslot
         self.env_cars = list()
-        self.mean_fps = None
-        self.time_counter = 0
 
-        self.user_is_done = False
         self.max_frame = max(self._df_by_timeslot[self._timeslot]['Frame ID'])
         self.vehicles_history_ids = set()
 
-    def step(self) -> (List[NGSimCar], bool):
+    def step(self) -> List[NGSimCar]:
         assert self.frame < self.max_frame
 
         df = self._df_by_timeslot[self._timeslot]
@@ -311,7 +314,7 @@ def step(self) -> (List[NGSimCar], bool):
             this_vehicle = df['Vehicle ID'] == vehicle_id
             car_df = df[this_vehicle & now_and_on]
             if len(car_df) < self.smoothing_window + 1: continue
-            car = NGSimCar(car_df, self.offset, self.smoothing_window)
+            car = NGSimCar(car_df, self.offset, kernel=self.smoothing_window, mapper=self._mapper)
             self.env_cars.append(car)
         self.vehicles_history_ids |= vehicles_ids  # union set operation
 
@@ -325,4 +328,4 @@ def step(self) -> (List[NGSimCar], bool):
 
         self.frame += 1
 
-        return self.env_cars
+        return [v.as_real_traffic_car() for v in self.env_cars]

carla_real_traffic_scenarios/ngsim/ngsim_carla_sync.py → carla_real_traffic_scenarios/utils/carla.py

@@ -1,73 +1,77 @@
 import logging
+from typing import Dict, Optional, NamedTuple
+
 import random
-from typing import Dict, Optional
 
 import carla
-
-from carla_real_traffic_scenarios.ngsim import NGSimDataset
-from carla_real_traffic_scenarios.ngsim.cords_mapping import MAPPER_BY_NGSIM_DATASET
-from carla_real_traffic_scenarios.ngsim.ngsim_recording import NGSimCar
 from carla_real_traffic_scenarios.utils.collections import smallest_by
 from carla_real_traffic_scenarios.utils.geometry import jaccard_rectangles
-from carla_real_traffic_scenarios.utils.transforms import Transform
-from carla_real_traffic_scenarios.vehicles import VehicleModel, VEHICLES, VEHICLE_BY_TYPE_ID
+from carla_real_traffic_scenarios.utils.transforms import Transform, Vector3
+from carla_real_traffic_scenarios.vehicles import VehicleModel, VEHICLES
 
 LOGGER = logging.getLogger(__name__)
 
 
-class NGSimVehiclesInCarla:
+class RealTrafficVehicle(NamedTuple):
+    id: int
+    type_id: str
+    width_m: float
+    length_m: float
+    transform: Transform
+    speed: float
+    debug: Optional[str]
+
+
+class RealTrafficVehiclesInCarla:
 
-    def __init__(self, client: carla.Client, world: carla.World, ngsim_dataset: NGSimDataset):
+    def __init__(self, client: carla.Client, world: carla.World):
         self._vehicle_by_vehicle_id: Dict[int, carla.Vehicle] = {}
         self._client = client
         self._world = world
-        self._ignored_ngsim_vehicle_ids = set()
-        self._mapper = MAPPER_BY_NGSIM_DATASET[ngsim_dataset]
+        self._ignored_real_traffic_vehicle_ids = set()
 
     def step(self, vehicles):
         commands = []
 
-        ngsim_v: NGSimCar
-        for ngsim_v in vehicles:
-            if ngsim_v.id in self._ignored_ngsim_vehicle_ids:
+        real_vehicle: RealTrafficVehicle
+        for real_vehicle in vehicles:
+            if real_vehicle.id in self._ignored_real_traffic_vehicle_ids:
                 continue
 
-            if ngsim_v.id in self._vehicle_by_vehicle_id:
-                carla_v = self._vehicle_by_vehicle_id[ngsim_v.id]
-                v_data = VEHICLE_BY_TYPE_ID[carla_v.type_id]
-                t = self._mapper.ngsim_to_carla(ngsim_v.get_transform(), carla_v.get_transform().location.z,
-                                                v_data.rear_axle_offset)
-                commands.append(carla.command.ApplyTransform(carla_v, t.as_carla_transform()))
+            target_transform = real_vehicle.transform  # transform in carla coordinates
+            if real_vehicle.id in self._vehicle_by_vehicle_id:
+                carla_vehicle = self._vehicle_by_vehicle_id[real_vehicle.id]
+                target_transform = Transform(target_transform.position.with_z(carla_vehicle.get_transform().location.z),
+                                             target_transform.orientation)
+                commands.append(carla.command.ApplyTransform(carla_vehicle, target_transform.as_carla_transform()))
             else:
-                model = find_best_matching_model(ngsim_v)
-
-                if model is None:
-                    LOGGER.debug(f"Not found matching vehicle model for vehicle {ngsim_v}")
-                    continue
-
-                target_transform = self._mapper.ngsim_to_carla(ngsim_v.get_transform(), model.z_offset,
-                                                               model.rear_axle_offset)
                 spawn_transform = Transform(target_transform.position.with_z(500), target_transform.orientation)
-                vehicle_blueprint = self._get_vehicle_blueprint(model.type_id)
-                vehicle_blueprint.set_attribute('role_name', 'ngsim_replay')
-                carla_v = self._world.try_spawn_actor(vehicle_blueprint, spawn_transform.as_carla_transform())
-                if carla_v is None:
+                vehicle_blueprint = self._get_vehicle_blueprint(real_vehicle.type_id)
+                vehicle_blueprint.set_attribute('role_name', 'real_traffic_replay')
+                carla_vehicle = self._world.try_spawn_actor(vehicle_blueprint, spawn_transform.as_carla_transform())
+                if carla_vehicle is None:
                     LOGGER.info(
-                        f"Error spawning vehicle with id {ngsim_v.id}. Ignoring it now in the future. Model: {model.type_id}.")
+                        f"Error spawning vehicle with id {real_vehicle.id}. "
+                        f"Ignoring it now in the future. Model: {real_vehicle.type_id}.")
                     # Without ignoring such vehicles till the end of episode a vehicle might suddenly appears mid-road
                     # in future frames
-                    self._ignored_ngsim_vehicle_ids.add(ngsim_v.id)
+                    self._ignored_real_traffic_vehicle_ids.add(real_vehicle.id)
                     continue
                 commands.append(
-                    carla.command.ApplyTransform(carla_v, target_transform.as_carla_transform())
+                    carla.command.ApplyTransform(carla_vehicle, target_transform.as_carla_transform())
                 )
-                self._vehicle_by_vehicle_id[ngsim_v.id] = carla_v
+                self._vehicle_by_vehicle_id[real_vehicle.id] = carla_vehicle
+
+            if real_vehicle.debug:
+                self._world.debug.draw_string((target_transform.position + Vector3(2, 0, 4)).as_carla_location(),
+                                              str(real_vehicle.debug))
 
             now_vehicle_ids = {v.id for v in vehicles}
             previous_vehicles_ids = set(self._vehicle_by_vehicle_id.keys())
 
             for to_remove_id in previous_vehicles_ids - now_vehicle_ids:
-                self._vehicle_by_vehicle_id[to_remove_id].destroy()
+                actor = self._vehicle_by_vehicle_id[to_remove_id]
+                commands.append(carla.command.DestroyActor(actor.id))
                 del self._vehicle_by_vehicle_id[to_remove_id]
 
         # TODO batch spawn and batch destroy
@@ -89,25 +93,26 @@ def close(self):
                 v.destroy()
 
 
-def find_best_matching_model(ngsim_car: NGSimCar) -> Optional[VehicleModel]:
+def find_best_matching_model(vehicle_width_m, vehicle_length_m) -> Optional[VehicleModel]:
     USE_STRICTLY_SMALLER = False
 
     if USE_STRICTLY_SMALLER:
-        # using stricly smaller models ensures that there will be no collisions
+        # using strictly smaller models ensures that there will be no collisions
         models = [
             m for m in VEHICLES if
-            m.bounding_box.extent.x * 2 < ngsim_car.length_m and m.bounding_box.extent.y * 2 < ngsim_car.width_m
+            m.bounding_box.extent.x * 2 < vehicle_length_m and
+            m.bounding_box.extent.y * 2 < vehicle_width_m
         ]
     else:
         models = list(VEHICLES)
 
     if len(models) == 0:
         return None
 
-    def calc_fitness(ngsim_car: NGSimCar, vehicle_model: VehicleModel):
+    def calc_fitness(vehicle_model: VehicleModel):
         return jaccard_rectangles(
-            ngsim_car.length_m, ngsim_car.width_m,
+            vehicle_length_m, vehicle_width_m,
             vehicle_model.bounding_box.extent.x * 2, vehicle_model.bounding_box.extent.y * 2
         )
 
-    return smallest_by(models, lambda m: -calc_fitness(ngsim_car, m))
+    return smallest_by(models, lambda m: -calc_fitness(m))

carla_real_traffic_scenarios/utils/transforms.py

@@ -110,10 +110,6 @@ def as_carla_rotation(self):
         yaw = math.atan2(self.y, self.x) * 180 / math.pi
         return carla.Rotation(yaw=float(yaw), roll=float(0), pitch=float(0))
 
-    @staticmethod
-    def from_carla_orientation(orientation):
-        return Vector2(orientation.x, orientation.y)
-
     @staticmethod
     def from_carla_rotation(rotation):
         yaw_rad = rotation.yaw * math.pi / 180

example/example_replay_ngsim_in_carla.py

@@ -1,9 +1,9 @@
 import carla
 
 from carla_real_traffic_scenarios import DT
-from carla_real_traffic_scenarios.ngsim.ngsim_carla_sync import NGSimVehiclesInCarla
+from carla_real_traffic_scenarios.ngsim import NGSimDatasets, US101Timeslots
 from carla_real_traffic_scenarios.ngsim.ngsim_recording import NGSimRecording
-from carla_real_traffic_scenarios.ngsim import NGSimDatasets, I80Timeslots, US101Timeslots
+from carla_real_traffic_scenarios.utils.carla import RealTrafficVehiclesInCarla
 
 
 def main():
@@ -25,12 +25,7 @@ def main():
         settings.fixed_delta_seconds = DT
         world.apply_settings(settings)
 
-        ngsim_vehicles_in_carla = NGSimVehiclesInCarla(
-            carla_client,
-            world,
-            ngsim_dataset
-        )
-
+        ngsim_vehicles_in_carla = RealTrafficVehiclesInCarla(carla_client, world)
         for _ in range(10000):
             vehicles = ngsim_recording.step()
             ngsim_vehicles_in_carla.step(vehicles)