Extract carla sync code

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,9 +7,9 @@
 
 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.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
@@ -84,7 +84,7 @@ def on_collided(e):
 
         self._lane_change: LaneChangeInstant = random.choice(self._lane_change_instants)
 
-        self._ngsim_vehicles_in_carla = NGSimVehiclesInCarla(self._client, self._world)
+        self._ngsim_vehicles_in_carla = RealTrafficVehiclesInCarla(self._client, self._world)
         self._target_alignment_counter = 0
 
         self._previous_chauffeur_command = self._lane_change.chauffeur_command
@@ -97,9 +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]
 
-        t = agent_ngsim_vehicle.get_transform()
+        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,20 +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
-from carla_real_traffic_scenarios.ngsim.cords_mapping import MAPPER_BY_NGSIM_DATASET
-from carla_real_traffic_scenarios.ngsim.ngsim_carla_sync import find_best_matching_model
+    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.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
 
@@ -186,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
@@ -231,7 +230,7 @@ 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.name]
+        self._mapper = MAPPER_BY_NGSIM_DATASET[ngsim_dataset]
 
     def _init_df(self, data_dir, x_offset_meters):
         self.lane_change_instants = []
@@ -329,4 +328,4 @@ def step(self) -> List[NGSimCar]:
 
         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,9 +1,9 @@
 import logging
+from typing import Dict, Optional, NamedTuple
+
 import random
-from typing import Dict, Optional
 
 import carla
-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
@@ -12,45 +12,54 @@
 LOGGER = logging.getLogger(__name__)
 
 
-class NGSimVehiclesInCarla:
+class RealTrafficVehicle(NamedTuple):
+    id: int
+    type_id: str
+    width_m: float
+    length_m: float
+    transform: Transform
+    speed: float
+
+
+class RealTrafficVehiclesInCarla:
 
     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._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
 
-            target_transform = ngsim_v.get_transform()  # transform in carla coordinates
-            if ngsim_v.id in self._vehicle_by_vehicle_id:
-                carla_v = self._vehicle_by_vehicle_id[ngsim_v.id]
-                target_transform = Transform(target_transform.position.with_z(carla_v.get_transform().location.z),
+            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_v, target_transform.as_carla_transform()))
+                commands.append(carla.command.ApplyTransform(carla_vehicle, target_transform.as_carla_transform()))
             else:
                 spawn_transform = Transform(target_transform.position.with_z(500), target_transform.orientation)
-                vehicle_blueprint = self._get_vehicle_blueprint(ngsim_v.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. "
-                        f"Model: {ngsim_v.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
 
             now_vehicle_ids = {v.id for v in vehicles}
             previous_vehicles_ids = set(self._vehicle_by_vehicle_id.keys())
@@ -83,10 +92,11 @@ def find_best_matching_model(vehicle_width_m, vehicle_length_m) -> Optional[Vehi
     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 < vehicle_length_m and m.bounding_box.extent.y * 2 < vehicle_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)

example/example_replay_ngsim_in_carla.py

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