Browse Source

feat: Implement Deterministic Showcase Suite with Asynchronous Pipeline and Auto-MCAP

This commit represents a major architectural upgrade to the CARLA ADAS simulation project, transitioning from stochastic Traffic Manager behavior to a fully deterministic, high-impact demo environment.

1. Deterministic Scenario Architecture
New Showcase Scenario: Implemented
scenarios/showcase.py
 for a manual "Left-Turn Across Path" (LTAP) maneuver.
Physics Velocity Injection: Overrode engine/friction variance by directly setting set_target_velocity for NPC actors at 35 km/h.
Multi-Stage Waypoint Guidance: Integrated EGO_MID and P1_MID transition points to ensure realistic, cinematic turning radii and reliable collision-avoidance timing.
Manual Steering Controller: Developed a proportional steering-to-target helper for all actors.

2. Sensor & Data Pipeline
Dual-Camera Support: Integrated a second Third-Person Perspective (TPP) camera.
Radar Serialization Fix: Implemented trigonometric conversion of raw spherical coordinates (Depth, Azimuth, Altitude) into Cartesian XYZ + Velocity 16-byte PointClouds for Foxglove Studio.
Synchronization: Aligned all sensor data (Dash Cam, TPP Cam, Radar, Lidar) at the orchestrator level.

3. Performance & Asynchronous I/O
Asynchronous Recorder: Refactored
src/recorder.py
 to use ThreadPoolExecutor, offloading cv2.imwrite operations to background threads to eliminate simulation stutter.
Rapid Iteration: Added --no-record global flag to
main.py
 for sub-second iteration during coordinate tuning.

4. Automation & Seamless UX
Auto-MCAP conversion: Integrated
data_to_mcap.py
 as a post-simulation callback in
main.py
.
Auto-Video Stitching: Implement automatic .mp4 generation for both camera views using OpenCV's VideoWriter.
Weather/Time CLI: Added --weather flag with presets (Clear, Rain, Sunset, Night).
Console Sync: Integrated tqdm for professional progress tracking and refactored scenario logging to use pbar.write() to prevent terminal flickering/staircasing.

5. Dependency Updates
Added tqdm as a core dependency for enhanced console visualization.
1843_integration
RUSHIL AMBARISH KADU 2 months ago
parent
commit
15fbf8e31d
  1. 2
      config.py
  2. 39
      data_to_mcap.py
  3. 79
      intel/showcase.md
  4. 1
      scenarios/__init__.py
  5. 2
      scenarios/base.py
  6. 2
      scenarios/braking.py
  7. 2
      scenarios/cutin.py
  8. 2
      scenarios/obstacle.py
  9. 132
      scenarios/showcase.py
  10. 22
      scripts/get_pos.py
  11. 29
      scripts/spawn_at_spectator.py
  12. 99
      src/main.py
  13. 69
      src/recorder.py
  14. 28
      src/sensors.py

2
config.py

@ -1,5 +1,5 @@
# Simulation settings
FPS = 20
FPS = 30
DELTA_SECONDS = 1.0 / FPS
# Camera settings

39
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)

79
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*

1
scenarios/__init__.py

@ -1 +1,2 @@
# scenarios package
from .showcase import ShowcaseScenario

2
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.

2
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

2
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

2
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

132
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()

22
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()

29
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()

99
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

69
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."""

28
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()

Loading…
Cancel
Save