diff --git a/config.py b/config.py index 89e8599..95c4181 100644 --- a/config.py +++ b/config.py @@ -1,5 +1,5 @@ # Simulation settings -FPS = 20 +FPS = 30 DELTA_SECONDS = 1.0 / FPS # Camera settings diff --git a/data_to_mcap.py b/data_to_mcap.py index fa70968..c030c18 100644 --- a/data_to_mcap.py +++ b/data_to_mcap.py @@ -84,6 +84,7 @@ def convert_folder(folder_path): # Register Channels camera_channel_id = writer.register_channel(topic="/camera", message_encoding="json", schema_id=camera_schema_id) + camera_tpp_channel_id = writer.register_channel(topic="/camera_tpp", message_encoding="json", schema_id=camera_schema_id) lidar_channel_id = writer.register_channel(topic="/lidar", message_encoding="json", schema_id=lidar_schema_id) pose_channel_id = writer.register_channel(topic="/ego_pose", message_encoding="json", schema_id=pose_schema_id) radar_channel_id = writer.register_channel(topic="/radar", message_encoding="json", schema_id=lidar_schema_id) @@ -117,6 +118,20 @@ def convert_folder(folder_path): } writer.add_message(camera_channel_id, log_time=ts_ns, data=json.dumps(cam_msg).encode(), publish_time=ts_ns) + # CAMERA (TPP) + if "camera_tpp" in frame: + camera_tpp_path = os.path.join(folder_path, "camera_tpp", frame["camera_tpp"]) + if os.path.exists(camera_tpp_path): + with open(camera_tpp_path, "rb") as img_f: + img_bytes = img_f.read() + cam_tpp_msg = { + "timestamp": {"sec": ts_sec, "nsec": ts_nsec}, + "frame_id": "ego_vehicle", + "format": "png" if frame["camera_tpp"].endswith(".png") else "jpeg", + "data": base64.b64encode(img_bytes).decode("ascii") + } + writer.add_message(camera_tpp_channel_id, log_time=ts_ns, data=json.dumps(cam_tpp_msg).encode(), publish_time=ts_ns) + # EGO POSE writer.add_message(pose_channel_id, log_time=ts_ns, data=json.dumps(ego_world_pose).encode(), publish_time=ts_ns) @@ -139,13 +154,29 @@ def convert_folder(folder_path): if os.path.exists(radar_path): r_data = np.load(radar_path) if r_data.size > 0: - dist, az, alt = r_data[:, 0], -r_data[:, 1], r_data[:, 2] + # r_data = [depth, azimuth, altitude, velocity] + # We negate azimuth to convert from CARLA (Right-handed for Y) + # note: CARLA is actually LHS (X-fwd, Y-right, Z-up) + # ROS is RHS (X-fwd, Y-left, Z-up) -> Negating Y converts it. + dist, az, alt, vel = r_data[:, 0], -r_data[:, 1], r_data[:, 2], r_data[:, 3] + xr, yr, zr = dist*np.cos(az)*np.cos(alt), dist*np.sin(az)*np.cos(alt), dist*np.sin(alt) + + # Stack X, Y, Z, and Velocity (4 floats = 16 bytes stride) + radar_points = np.stack([xr, yr, zr, vel], axis=1).astype(np.float32) + radar_msg = { "timestamp": {"sec": ts_sec, "nsec": ts_nsec}, - "frame_id": "ego_vehicle", "pose": identity_pose, "point_stride": 12, - "fields": [{"name":"x","offset":0,"type":7}, {"name":"y","offset":4,"type":7}, {"name":"z","offset":8,"type":7}], - "data": base64.b64encode(np.stack([xr,yr,zr],axis=1).astype(np.float32).tobytes()).decode("ascii") + "frame_id": "ego_vehicle", + "pose": identity_pose, + "point_stride": 16, + "fields": [ + {"name":"x","offset":0,"type":7}, + {"name":"y","offset":4,"type":7}, + {"name":"z","offset":8,"type":7}, + {"name":"velocity","offset":12,"type":7} + ], + "data": base64.b64encode(radar_points.tobytes()).decode("ascii") } writer.add_message(radar_channel_id, log_time=ts_ns, data=json.dumps(radar_msg).encode(), publish_time=ts_ns) diff --git a/intel/showcase.md b/intel/showcase.md new file mode 100644 index 0000000..b17eeff --- /dev/null +++ b/intel/showcase.md @@ -0,0 +1,79 @@ +# CARLA Custom Scenario Design: Deterministic Showcase Suite (Intel) + +This document serves as a "Deep Dive" into the design decisions, technical implementation, and lessons learned during the development of the **Deterministic Showcase Suite**. Use this as a reference for creating future high-impact ADAS demo scenarios. + +--- + +## πŸ—οΈ 1. Architecture: Manual vs. Traffic Manager + +**Standard Approach**: Using `ego_vehicle.set_autopilot(True)` and relying on Traffic Manager (TM). +- **Pros**: Easy to set up, handles traffic rules automatically. +- **Cons**: Unpredictable. Random lane changes, braking for minor obstacles, and "softening" of turns makes for poor demo repeatability. + +**Deterministic Approach (Showcase)**: Manual coordinate-based control. +- **Core Strategy**: Define precise spawn points (X, Y, Z) and use a custom steering-to-target loop. +- **Why**: Ensures that the "Left-Turn Across Path" (LTAP) maneuver happens at the *exact same frame* every time, regardless of noise or simulation variance. + +--- + +## 🏎️ 2. The "Secret Sauce": Physics Velocity Injection + +One of the biggest challenges in CARLA is consistent timing. Rolling friction, gear shifts, and engine torque curves cause variability in speed. + +**The Fix**: Direct `set_target_velocity` override. +- **Implementation**: Instead of applying `throttle=1.0` and hoping for the best, we calculate the forward vector and force the velocity to exactly **9.72 m/s** (35 km/h). +- **Result**: The NPC reaches the intersection at the *exact same frame* every run, allowing us to tune the EGO's arrival time down to the millisecond. + +--- + +## πŸ—ΊοΈ 3. Multi-Stage Waypoint Guidance + +**The Issue**: Standard "Steer toward target" logic often leads to curb-clipping or unrealistic, jagged turns at intersections. CARLA's pathfinding is optimized for driving, not cinematic maneuvers. + +**The Solution**: Intermediate `MID` points. +- **Strategy**: Instead of pointing the NPC directly at the final target, we created a `P1_MID` waypoint. +- **Outcome**: This forces the vehicle to stay in its lane longer and take a wider, "swept" turn, which looks significantly more professional and avoids clipping the junction geometry. + +--- + +## πŸ“‘ 4. Radar Calibration & Serialization + +**The Caveat**: CARLA's Radar sensor exports data in **Spherical Coordinates** (Depth, Azimuth, Altitude, Velocity). Most visualization tools (like Foxglove PointClouds) expect **Cartesian (XYZ)**. + +**The Math**: +- `x = depth * cos(alt) * cos(az)` +- `y = depth * cos(alt) * sin(az)` +- `z = depth * sin(alt)` +- **Note**: The `velocity` field must be included separately and requires a 16-byte point stride (XYZ + V) in the MCAP schema. + +--- + +## ⚑ 5. Performance & Caveats + +### Asynchronous I/O +- Always use `ThreadPoolExecutor` for image saving. Saving 720p PNGs synchronously will drop your simulation from 30 FPS to ~5 FPS, ruining the physics time-step. + +### The "Progress Bar Clash" +- When using `tqdm`, standard `print()` statements will "staircase" the bar and ruin the UI. +- **Fix**: Refactor scenarios to accept a `pbar` object and use `pbar.write(msg)` instead of `print(msg)`. + +### CARLA API Gotchas +- **Weather API**: The parameter for sun height is `sun_altitude_angle`, NOT `pitch`. Using the wrong name (or passing an `int` when a `float` is expected) will trigger a `Boost.Python` type error. +- **Sensor Sync**: Always fetch sensor data *after* `world.tick()` to ensure the frames match the metadata. + +--- + +## πŸš€ 6. The Automation Pipeline + +We turned a 2-step manual process into a single-step autonomous pipeline: +1. `main.py` runs the simulation. +2. `recorder.py` captures frames (Async) + generates `.mp4` previews. +3. `main.py` cleanup callback triggers `data_to_mcap.py` automatically. + +**Run Command**: +```powershell +python src/main.py --scenario showcase --frames 250 --weather ClearNoon +``` + +--- +*Created: March 2026 | Intel Repository β€” ADAS Development* diff --git a/scenarios/__init__.py b/scenarios/__init__.py index 7174a54..077eb30 100644 --- a/scenarios/__init__.py +++ b/scenarios/__init__.py @@ -1 +1,2 @@ # scenarios package +from .showcase import ShowcaseScenario diff --git a/scenarios/base.py b/scenarios/base.py index 9e7a7d4..c6eebd8 100644 --- a/scenarios/base.py +++ b/scenarios/base.py @@ -67,7 +67,7 @@ class ScenarioBase(ABC): """ @abstractmethod - def step(self, frame: int, ego_vehicle) -> None: + def step(self, frame_count: int, ego_vehicle, pbar=None) -> None: """ Per-tick logic executed inside the main simulation loop. diff --git a/scenarios/braking.py b/scenarios/braking.py index 036e408..13de74c 100644 --- a/scenarios/braking.py +++ b/scenarios/braking.py @@ -89,7 +89,7 @@ class BrakingScenario(ScenarioBase): f"Emergency brake at frame {self.BRAKE_FRAME}." ) - def step(self, frame: int, ego_vehicle) -> None: + def step(self, frame_count: int, ego_vehicle, pbar=None) -> None: if frame == self.BRAKE_FRAME and not self._braked: if self._lead_vehicle and self._lead_vehicle.is_alive: # Disengage TM autopilot and apply full brake diff --git a/scenarios/cutin.py b/scenarios/cutin.py index 01aa5b9..0b9b12a 100644 --- a/scenarios/cutin.py +++ b/scenarios/cutin.py @@ -84,7 +84,7 @@ class CutInScenario(ScenarioBase): f"{self.NPC_DISTANCE_M} m ahead. Cut-in at frame {self.CUTIN_FRAME}." ) - def step(self, frame: int, ego_vehicle) -> None: + def step(self, frame_count: int, ego_vehicle, pbar=None) -> None: if frame == self.CUTIN_FRAME and not self._cut_triggered: if self._npc and self._npc.is_alive: # force_lane_change: True = left, False = right diff --git a/scenarios/obstacle.py b/scenarios/obstacle.py index e0e782b..58f89e6 100644 --- a/scenarios/obstacle.py +++ b/scenarios/obstacle.py @@ -88,7 +88,7 @@ class ObstacleScenario(ScenarioBase): f"{self.OBSTACLE_DISTANCE_M} m ahead of ego." ) - def step(self, frame: int, ego_vehicle) -> None: + def step(self, frame_count: int, ego_vehicle, pbar=None) -> None: # Static scenario β€” no per-frame logic required pass diff --git a/scenarios/showcase.py b/scenarios/showcase.py new file mode 100644 index 0000000..a53f35c --- /dev/null +++ b/scenarios/showcase.py @@ -0,0 +1,132 @@ +""" +scenarios/showcase.py +--------------------- +Custom high-impact demo scene: Left Turn Across Path (LTAP). +Built step-by-step for full control and learning. + +Phase 1: Ego Foundation +""" + +import carla +import math +from scenarios.base import ScenarioBase + +class ShowcaseScenario(ScenarioBase): + @property + def name(self): + return "showcase" + + def setup(self, world, ego_vehicle, traffic_manager) -> None: + """ + Step 11: Deep Intersection Guidance for Ego. + """ + self._world = world + self._ego = ego_vehicle + self._tm = traffic_manager + + # User Coordinates + self.P1 = carla.Transform(carla.Location(x=101.573, y=-8.183, z=0.5), carla.Rotation(yaw=98.4)) + self.P2 = carla.Transform(carla.Location(x=107.412, y=45.309, z=0.5), carla.Rotation(yaw=-87.7)) + self.P3 = carla.Location(x=82.312, y=24.801, z=0.5) + + # NPC Waypoint Guidance + self.P1_MID = carla.Location(x=101.573, y=18.0, z=0.5) + self._npc_reached_mid = False + + # NEW: EGO Mid-point to force a larger turn radius + # Aligned with P2, but 10m further south into intersection + self.EGO_MID = carla.Location(x=107.412, y=32.0, z=0.5) + self._ego_reached_mid = False + + # 1. Setup Ego + self._ego.set_transform(self.P2) + self._ego.set_autopilot(False) + + # 2. Spawn NPC + bp_lib = world.get_blueprint_library() + npc_bp = bp_lib.filter("vehicle.nissan.micra")[0] + self._npc = world.try_spawn_actor(npc_bp, self.P1) + + if self._npc: + self._actors.append(self._npc) + self._npc.set_autopilot(False) + print(f"[SUCCESS] Actors ready. Multi-stage pathing ENABLED.") + + # 3. Visual Debug: Dimmed Boxes (Commented out for final presentation) + # world.debug.draw_box( + # carla.BoundingBox(self.P3 + carla.Location(z=1), carla.Vector3D(0.5, 0.5, 1.0)), + # carla.Rotation(), thickness=0.1, color=carla.Color(178, 0, 0), life_time=20.0 + # ) + # world.debug.draw_box( + # carla.BoundingBox(self.P1_MID + carla.Location(z=1), carla.Vector3D(0.5, 0.5, 1.0)), + # carla.Rotation(), thickness=0.1, color=carla.Color(0, 178, 0), life_time=20.0 + # ) + # world.debug.draw_box( + # carla.BoundingBox(self.EGO_MID + carla.Location(z=1), carla.Vector3D(0.5, 0.5, 1.0)), + # carla.Rotation(), thickness=0.1, color=carla.Color(0, 0, 178), life_time=20.0 + # ) + + def _get_steering_to_target(self, actor, target_loc): + t = actor.get_transform() + v = target_loc - t.location + target_yaw = math.degrees(math.atan2(v.y, v.x)) + delta_yaw = target_yaw - t.rotation.yaw + while delta_yaw > 180: delta_yaw -= 360 + while delta_yaw < -180: delta_yaw += 360 + return max(-1.0, min(1.0, delta_yaw / 60.0)) + + def step(self, frame: int, ego_vehicle, pbar=None) -> None: + """ + Step 11: Multi-stage steering logic. + """ + # --- Ego Control --- + e_loc = ego_vehicle.get_location() + if not self._ego_reached_mid: + e_target = self.EGO_MID + if e_loc.distance(self.EGO_MID) < 2.5: + self._ego_reached_mid = True + msg = "[DEBUG] Ego reached MID point. Swinging into turn." + if pbar: pbar.write(msg) + else: print(msg) + else: + e_target = self.P3 + + e_dist = e_loc.distance(e_target) + ego_steer = self._get_steering_to_target(ego_vehicle, e_target) if e_dist > 1.0 else 0.0 + ego_vehicle.apply_control(carla.VehicleControl(throttle=0.4, steer=ego_steer)) + + # --- NPC Control --- + n_steer, n_dist = 0.0, 0.0 + if hasattr(self, "_npc") and self._npc and self._npc.is_alive: + n_loc = self._npc.get_location() + + if not self._npc_reached_mid: + n_target = self.P1_MID + if n_loc.distance(self.P1_MID) < 2.5: + self._npc_reached_mid = True + msg = "[DEBUG] NPC reached MID point. Switching to P3." + if pbar: pbar.write(msg) + else: print(msg) + else: + n_target = self.P3 + + n_dist = n_loc.distance(n_target) + n_steer = self._get_steering_to_target(self._npc, n_target) if n_dist > 1.0 else 0.0 + + # Physics Injection (35 km/h) + fwd = self._npc.get_transform().get_forward_vector() + self._npc.set_target_velocity(fwd * 9.72) + self._npc.apply_control(carla.VehicleControl(throttle=1.0, steer=n_steer)) + + # Verbose Logging + if frame % 10 == 0: + e_vel = ego_vehicle.get_velocity() + e_speed = 3.6 * math.sqrt(e_vel.x**2 + e_vel.y**2 + e_vel.z**2) + msg = f"[FRAME {frame:03d}] EGO (spd={e_speed:.1f}kph) target={'MID' if not self._ego_reached_mid else 'P3'} | NPC target={'MID' if not self._npc_reached_mid else 'P3'}" + if pbar: + pbar.write(msg) + else: + print(msg) + + def cleanup(self): + self._destroy_actors() diff --git a/scripts/get_pos.py b/scripts/get_pos.py new file mode 100644 index 0000000..12bbbbc --- /dev/null +++ b/scripts/get_pos.py @@ -0,0 +1,22 @@ +import carla + +def get_pos(): + client = carla.Client("localhost", 2000) + client.set_timeout(10.0) + world = client.get_world() + spectator = world.get_spectator() + t = spectator.get_transform() + loc = t.location + rot = t.rotation + + print("\n" + "="*40) + print("CURRENT SPECTATOR POSITION") + print("="*40) + print(f"Location: x={loc.x:.3f}, y={loc.y:.3f}, z={loc.z:.3f}") + print(f"Rotation: pitch={rot.pitch:.3f}, yaw={rot.yaw:.3f}, roll={rot.roll:.3f}") + print("="*40) + print("\nCopy-paste this into your script:") + print(f"transform = carla.Transform(carla.Location(x={loc.x:.3f}, y={loc.y:.3f}, z={loc.z:.3f}), carla.Rotation(pitch={rot.pitch:.3f}, yaw={rot.yaw:.3f}, roll={rot.roll:.3f}))") + +if __name__ == "__main__": + get_pos() diff --git a/scripts/spawn_at_spectator.py b/scripts/spawn_at_spectator.py new file mode 100644 index 0000000..14a0f47 --- /dev/null +++ b/scripts/spawn_at_spectator.py @@ -0,0 +1,29 @@ +import carla +import argparse + +def spawn_here(): + parser = argparse.ArgumentParser() + parser.add_argument("--model", default="vehicle.nissan.micra") + args = parser.parse_args() + + client = carla.Client("localhost", 2000) + client.set_timeout(10.0) + world = client.get_world() + + spectator = world.get_spectator() + t = spectator.get_transform() + + # Lower it slightly so it doesn't drop from the sky, + # but stays above ground + t.location.z -= 1.0 + + bp = world.get_blueprint_library().filter(args.model)[0] + actor = world.try_spawn_actor(bp, t) + + if actor: + print(f"[SUCCESS] Spawned {args.model} at spectator location.") + else: + print("[ERROR] Spawn failed. Collision or invalid location.") + +if __name__ == "__main__": + spawn_here() diff --git a/src/main.py b/src/main.py index c538412..56f05e3 100644 --- a/src/main.py +++ b/src/main.py @@ -22,6 +22,7 @@ sys.path.append(os.path.dirname(os.path.dirname(__file__))) import config from scenario_loader import load_scenario, list_scenarios +from data_to_mcap import convert_folder # ----------------------------------------------------------------------- @@ -44,6 +45,23 @@ def parse_args(): default=None, help="Override config.MAX_FRAMES for this run" ) + parser.add_argument( + "--spawn-point", "-p", + type=int, + default=None, + help="Index of the spawn point to use for ego vehicle" + ) + parser.add_argument( + "--no-record", + action="store_true", + help="Disable image and metadata recording for faster iteration" + ) + parser.add_argument( + "--weather", "-w", + type=str, + default="ClearNoon", + help="Weather preset (ClearNoon, SoftRain, Rain, Sunset, Night, Wet, etc.)" + ) parser.add_argument( "--list-scenarios", "-l", action="store_true", @@ -84,6 +102,26 @@ def main(): client.set_timeout(10.0) world = client.get_world() + + # Apply Weather + weather_presets = { + "Clear": carla.WeatherParameters.ClearNoon, + "Cloudy": carla.WeatherParameters.CloudyNoon, + "Wet": carla.WeatherParameters.WetNoon, + "SoftRain": carla.WeatherParameters.SoftRainNoon, + "Rain": carla.WeatherParameters.HardRainNoon, + "Sunset": carla.WeatherParameters.ClearSunset, + "Night": carla.WeatherParameters(sun_altitude_angle=-90.0, cloudiness=0.0, precipitation=10.0, precipitation_deposits=10.0, wind_intensity=0.0, fog_density=0.0, fog_distance=0.0, wetness=0.0) + } + # Match partial string (e.g. "Rain" matches "HardRainNoon") + chosen_weather = carla.WeatherParameters.ClearNoon + for k, v in weather_presets.items(): + if k.lower() in args.weather.lower(): + chosen_weather = v + break + world.set_weather(chosen_weather) + print(f"[INFO] Applied weather: {args.weather}") + blueprint_library = world.get_blueprint_library() map_ = world.get_map() @@ -114,11 +152,24 @@ def main(): spawn_points = map_.get_spawn_points() ego_vehicle = None - for sp in spawn_points: - ego_vehicle = world.try_spawn_actor(vehicle_bp, sp) - if ego_vehicle: - print("[INFO] Ego spawned successfully") - break + + # Try specified spawn point first if provided + if args.spawn_point is not None: + if args.spawn_point < len(spawn_points): + sp = spawn_points[args.spawn_point] + ego_vehicle = world.try_spawn_actor(vehicle_bp, sp) + if ego_vehicle: + print(f"[INFO] Ego spawned at requested spawn point index {args.spawn_point}") + else: + print(f"[WARN] Spawn point index {args.spawn_point} out of range (max {len(spawn_points)-1})") + + # Fallback to searching if not specified or failed + if ego_vehicle is None: + for i, sp in enumerate(spawn_points): + ego_vehicle = world.try_spawn_actor(vehicle_bp, sp) + if ego_vehicle: + print(f"[INFO] Ego spawned at spawn point index {i}") + break if ego_vehicle is None: raise RuntimeError("Failed to spawn ego vehicle") @@ -134,7 +185,10 @@ def main(): sensor_manager = SensorManager(world, blueprint_library, ego_vehicle) sensor_manager.spawn_sensors() - recorder = Recorder(scenario_name=scenario.name) + recorder = None + if not args.no_record: + recorder = Recorder(scenario_name=scenario.name) + spectator = world.get_spectator() # ------------------------------------------------------------------ @@ -149,38 +203,51 @@ def main(): # 7. Main simulation loop # ------------------------------------------------------------------ try: + # Progress Tracking + from tqdm import tqdm + pbar = tqdm(total=max_frames, desc=f"SIMULATING: {scenario.name}", unit=" frames", colour='cyan') + while frame_count < max_frames: world.tick() frame_count += 1 - - cam, radar, lidar = sensor_manager.get_data() + + pbar.update(1) + + cam, cam_tpp, radar, lidar = sensor_manager.get_data() transform = ego_vehicle.get_transform() + # Third-person spectator view (matched to sensors.py) spectator.set_transform( carla.Transform( - transform.location + carla.Location(x=-6, z=3), - transform.rotation + transform.location + transform.get_forward_vector() * -5.5 + carla.Location(z=2.8), + carla.Rotation(pitch=transform.rotation.pitch - 15, yaw=transform.rotation.yaw, roll=transform.rotation.roll) ) ) # Merge scenario metadata into every frame record - extra_meta = scenario.get_scenario_metadata() - recorder.save(cam, radar, lidar, ego_vehicle, extra_meta=extra_meta) - - scenario.step(frame_count, ego_vehicle) + if recorder: + extra_meta = scenario.get_scenario_metadata() + recorder.save(cam, cam_tpp, radar, lidar, ego_vehicle, extra_meta=extra_meta) - print(f"[FRAME {frame_count:04d}/{max_frames}] [{scenario.name}] SAVED") + scenario.step(frame_count, ego_vehicle, pbar=pbar) # ------------------------------------------------------------------ # 8. Shutdown (always runs) # ------------------------------------------------------------------ finally: - print("[INFO] Shutting down...") + if pbar: + pbar.close() + print("\n\n[INFO] Simulation complete. Cleaning up...") scenario.cleanup() sensor_manager.destroy() ego_vehicle.destroy() - recorder.close() + if recorder: + recorder.close() + # AUTO-MCAP Step + if os.path.exists(recorder.base_path): + print(f"[AUTO-MCAP] Triggering seamless conversion for: {recorder.base_path}") + convert_folder(recorder.base_path) # Restore async mode settings.synchronous_mode = False diff --git a/src/recorder.py b/src/recorder.py index 2765e5e..76fcc35 100644 --- a/src/recorder.py +++ b/src/recorder.py @@ -3,6 +3,7 @@ import json import math import datetime from types import SimpleNamespace +from concurrent.futures import ThreadPoolExecutor import numpy as np import cv2 @@ -16,30 +17,48 @@ class Recorder: self.base_path = os.path.join(base_path, session_name) self.camera_path = os.path.join(self.base_path, "camera") + self.camera_tpp_path = os.path.join(self.base_path, "camera_tpp") self.radar_path = os.path.join(self.base_path, "radar") self.lidar_path = os.path.join(self.base_path, "lidar") self.meta_file = os.path.join(self.base_path, "frames.jsonl") os.makedirs(self.camera_path, exist_ok=True) + os.makedirs(self.camera_tpp_path, exist_ok=True) os.makedirs(self.radar_path, exist_ok=True) os.makedirs(self.lidar_path, exist_ok=True) self.frame_id = 0 self.meta_f = open(self.meta_file, "w") + + # Performance optimization: Background thread pool for image saving + # This prevents cv2.imwrite from blocking the main simulation tick + self.executor = ThreadPoolExecutor(max_workers=4) print(f"--- Recorder initialized. Saving data to: {self.base_path} ---") + def _async_save_image(self, path, img): + """Helper to save image in a background thread.""" + cv2.imwrite(path, img) + # ---------------- SAVE FRAME ---------------- - def save(self, cam, radar, lidar, vehicle, extra_meta: dict = None): + def save(self, cam, cam_tpp, radar, lidar, vehicle, extra_meta: dict = None): self.frame_id += 1 - # -------- CAMERA -------- + # -------- CAMERA (Dash) -------- img = np.frombuffer(cam.raw_data, dtype=np.uint8) img = img.reshape((cam.height, cam.width, 4))[:, :, :3] - cam_file = f"frame_{self.frame_id:06d}.png" cam_path = os.path.join(self.camera_path, cam_file) - cv2.imwrite(cam_path, img) + # Background write + self.executor.submit(self._async_save_image, cam_path, img) + + # -------- CAMERA (TPP) -------- + img_tpp = np.frombuffer(cam_tpp.raw_data, dtype=np.uint8) + img_tpp = img_tpp.reshape((cam_tpp.height, cam_tpp.width, 4))[:, :, :3] + cam_tpp_file = f"frame_{self.frame_id:06d}.png" + cam_tpp_path = os.path.join(self.camera_tpp_path, cam_tpp_file) + # Background write + self.executor.submit(self._async_save_image, cam_tpp_path, img_tpp) # -------- RADAR -------- radar_points = [] @@ -127,9 +146,10 @@ class Recorder: "x": transform.location.x, "y": transform.location.y, "z": transform.location.z, "yaw": transform.rotation.yaw }, - "camera": cam_file, - "radar": radar_file, - "lidar": lidar_file, + "camera": cam_file, + "camera_tpp": cam_tpp_file, + "radar": radar_file, + "lidar": lidar_file, "ground_truth": gt_actors, } @@ -142,8 +162,43 @@ class Recorder: # ---------------- CLEANUP ---------------- def close(self): + print(f"[INFO] Finalizing recording... Waiting for background image tasks.") + self.executor.shutdown(wait=True) + + # New: Generate MP4 videos automatically + self._generate_videos() + self.meta_f.close() + def _generate_videos(self): + """Stitch captured frames into .mp4 files for easy viewing.""" + print(f"[INFO] Generating video previews...") + + # We'll do this for both cameras + configs = [ + (self.camera_path, "maneuver_dash.mp4"), + (self.camera_tpp_path, "maneuver_tpp.mp4") + ] + + for folder, out_name in configs: + images = sorted([img for img in os.listdir(folder) if img.endswith(".png")]) + if not images: + continue + + first_frame = cv2.imread(os.path.join(folder, images[0])) + h, w, _ = first_frame.shape + + out_path = os.path.join(self.base_path, out_name) + # Using mp4v codec for broad compatibility + fourcc = cv2.VideoWriter_fourcc(*'mp4v') + out = cv2.VideoWriter(out_path, fourcc, 20.0, (w, h)) + + for img_name in images: + frame = cv2.imread(os.path.join(folder, img_name)) + out.write(frame) + out.release() + print(f" - Video saved: {out_name}") + # ---------------- HELPERS ---------------- def _get_actor_class(self, actor) -> str: """Categorise actor into broad ADAS classes.""" diff --git a/src/sensors.py b/src/sensors.py index 9bc1701..f0d03e1 100644 --- a/src/sensors.py +++ b/src/sensors.py @@ -21,11 +21,13 @@ class SensorManager: # Queues for synchronization self.camera_queue = queue.Queue() + self.camera_tpp_queue = queue.Queue() self.radar_queue = queue.Queue() self.lidar_queue = queue.Queue() def spawn_sensors(self): self._spawn_camera() + self._spawn_camera_tpp() self._spawn_radar() self._spawn_lidar() @@ -47,6 +49,23 @@ class SensorManager: self.camera.listen(lambda data: self.camera_queue.put(data)) + def _spawn_camera_tpp(self): + cam_bp = self.bp_lib.find("sensor.camera.rgb") + cam_bp.set_attribute("image_size_x", str(config.CAM_WIDTH)) + cam_bp.set_attribute("image_size_y", str(config.CAM_HEIGHT)) + cam_bp.set_attribute("fov", str(config.CAM_FOV)) + + transform = carla.Transform( + carla.Location(x=-5.5, z=2.8), + carla.Rotation(pitch=-15) + ) + + self.camera_tpp = self.world.spawn_actor( + cam_bp, transform, attach_to=self.vehicle + ) + + self.camera_tpp.listen(lambda data: self.camera_tpp_queue.put(data)) + # ---------------- RADAR ---------------- def _spawn_radar(self): radar_bp = self.bp_lib.find("sensor.other.radar") @@ -87,13 +106,14 @@ class SensorManager: # ---------------- FETCH SYNC DATA ---------------- def get_data(self): cam = self.camera_queue.get() + cam_tpp = self.camera_tpp_queue.get() radar = self.radar_queue.get() lidar = self.lidar_queue.get() # Strong sync check - assert cam.frame == radar.frame == lidar.frame, "Frame mismatch!" + assert cam.frame == cam_tpp.frame == radar.frame == lidar.frame, "Frame mismatch!" - return cam, radar, lidar + return cam, cam_tpp, radar, lidar # ---------------- CLEANUP ---------------- def destroy(self): @@ -101,6 +121,10 @@ class SensorManager: self.camera.stop() self.camera.destroy() + if self.camera_tpp: + self.camera_tpp.stop() + self.camera_tpp.destroy() + if self.radar: self.radar.stop() self.radar.destroy()