From 8ec04b881c61c9b4ddb3bd4d76c2ef84433f5cc3 Mon Sep 17 00:00:00 2001 From: rakadu1 Date: Wed, 1 Apr 2026 20:00:44 +0530 Subject: [PATCH] feat(shenron): Integrate C-SHENRON physics-based radar simulation - Transitions simulation to Model-Based Development (MBD) architecture. - Added semantic LiDAR sensor with high-fidelity point cloud recording. - Fixed '0 points' bug by implementing bit-casting for uint32 semantic tags (Tag 10 -> Metal). - Corrected antenna count hardcoding and range-slicing errors in radar processor. - Synchronized hardware configs between project settings and simulation engine. - Automated synthesis and MCAP serialization in src/main.py cleanup phase. - Verified rich 5-field point cloud output (x, y, z, velocity, magnitude) in showcase. --- .../lidar.py | 42 +++++++++++++++---- scripts/data_to_mcap.py | 2 +- scripts/generate_shenron.py | 2 +- scripts/verify_tags.py | 29 +++++++++++++ src/main.py | 9 +++- 5 files changed, 72 insertions(+), 12 deletions(-) create mode 100644 scripts/verify_tags.py diff --git a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/lidar.py b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/lidar.py index d4ab159..85c755e 100644 --- a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/lidar.py +++ b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/lidar.py @@ -19,15 +19,39 @@ def map_carla_semantic_lidar_latest(carla_sem_lidar_data): ''' Function to map material column in the collected carla ray_cast_shenron to shenron input ''' - # CARLA 0.9.16 [x, y, z, intensity, cos, obj, tag] - # We want [x, y, z, tag] - carla_sem_lidar_data_crop = carla_sem_lidar_data[:, (0, 1, 2, 6)] - temp_list = np.array([0, 4, 2, 0, 11, 5, 0, 0, 1, 8, 12, 3, 7, 10, 0, 1, 0, 12, 6, 0, 0, 0, 0]) - - col = temp_list[(carla_sem_lidar_data_crop[:, 3].astype(int))] - carla_sem_lidar_data_crop[:, 3] = col - - return carla_sem_lidar_data_crop + # CARLA 0.9.16 raw bits for [x, y, z, cos, obj, tag] + # We want [x, y, z, legacy_tag] + # Tag is at index 6 if padded, or index 5 in raw + tag_col_idx = 6 if carla_sem_lidar_data.shape[1] == 7 else 5 + + # CRITICAL: Recover real integer tags from the float32 bitstream + # Force float32 cast before view to ensure 1:1 element mapping + tags = carla_sem_lidar_data[:, tag_col_idx].astype(np.float32).view(np.uint32) + + # CARLA 0.9.16 -> Legacy team_code mapping (used by new_map_material) + # 0.9.16: 0:none, 1:building, 2:fence, 4:pedestrian, 10:vehicle, 11:wall, 12:traffic_sign, 13:sky, 14:ground, 15:bridge, 16:rail, 17:guardrail, 18:traffic_light, 19:static, 20:dynamic, 21:water, 22:terrain + # Legacy: 13:car, 11:pedestrian, 4:fence, 5:pole, 7:traffic_sign, 2:building, 3:wall, 1:sidewalk, 8:vegetation + + # We create a 256-long lookup table for all possible tags + mapping = np.zeros(256, dtype=int) + mapping[1] = 2 # Building + mapping[2] = 4 # Fence + mapping[4] = 11 # Pedestrian + mapping[5] = 5 # Pole + mapping[8] = 1 # Sidewalk + mapping[9] = 8 # Vegetation + mapping[10] = 13 # Vehicle + mapping[11] = 3 # Wall + mapping[12] = 7 # Traffic Sign + mapping[18] = 6 # Traffic Light + + legacy_tags = mapping[tags % 256] # bitwise safety + + # Return [x, y, z, legacy_tag] + res = np.zeros((carla_sem_lidar_data.shape[0], 4)) + res[:, 0:3] = carla_sem_lidar_data[:, 0:3] + res[:, 3] = legacy_tags + return res # def map_carla_semantic_lidar(carla_sem_lidar_data): # ''' diff --git a/scripts/data_to_mcap.py b/scripts/data_to_mcap.py index db850e7..7951086 100644 --- a/scripts/data_to_mcap.py +++ b/scripts/data_to_mcap.py @@ -191,7 +191,7 @@ def convert_folder(folder_path): writer.add_message(radar_channel_id, log_time=ts_ns, data=json.dumps(radar_msg).encode(), publish_time=ts_ns) # SHENRON RADAR - shenron_file = f"frame_{self.frame_id:06d}.npy" + shenron_file = f"frame_{int(frame['frame_id']):06d}.npy" shenron_path = os.path.join(folder_path, "shenron_radar", shenron_file) if os.path.exists(shenron_path): s_data = np.load(shenron_path) diff --git a/scripts/generate_shenron.py b/scripts/generate_shenron.py index 7b6174b..f5316bd 100644 --- a/scripts/generate_shenron.py +++ b/scripts/generate_shenron.py @@ -49,7 +49,7 @@ def process_session(session_path): if data.shape[1] == 6: # Pad with a dummy intensity column at index 3 # This aligns 'tag' to index 6 as expected by our lidar.py mapping - padded_data = np.zeros((data.shape[0], 7)) + padded_data = np.zeros((data.shape[0], 7), dtype=np.float32) padded_data[:, 0:3] = data[:, 0:3] # x, y, z padded_data[:, 4:7] = data[:, 3:6] # cos, obj, tag data = padded_data diff --git a/scripts/verify_tags.py b/scripts/verify_tags.py new file mode 100644 index 0000000..e469a3a --- /dev/null +++ b/scripts/verify_tags.py @@ -0,0 +1,29 @@ +import numpy as np +import os + +# Path to the failed session's lidar frame +frame_path = r'd:\CARLA\CARLA_0.9.16\PythonAPI\Fox\data\showcase_20260401_194412\lidar\frame_000001.npy' + +if not os.path.exists(frame_path): + print(f"File not found: {frame_path}") +else: + data = np.load(frame_path) + print(f"Data shape: {data.shape}") + + # Take the Tag column (index 5) + tags_float = data[:, 5] + + # Method 1: Simple cast (the wrong way) + tags_int_wrong = tags_float.astype(int) + print(f"Unique tags (wrong way): {np.unique(tags_int_wrong)}") + + # Method 2: Bit-casting (the right way) + # We must ensure it's a copy and contiguous for .view() to work as intended on a slice + tags_int_right = tags_float.copy().view(np.uint32) + print(f"Unique tags (right way): {np.unique(tags_int_right)}") + + # Method 3: Bit-casting on the whole row (even safer) + # CARLA [x:f32, y:f32, z:f32, cos:f32, obj:u32, tag:u32] + # If we read it all as f32, we can recover correctly + obj_ids = data[:, 4].copy().view(np.uint32) + print(f"Unique object IDs (right way): {len(np.unique(obj_ids))} unique IDs found.") diff --git a/src/main.py b/src/main.py index 0a9eee2..b1217aa 100644 --- a/src/main.py +++ b/src/main.py @@ -23,6 +23,8 @@ sys.path.append(os.path.dirname(os.path.dirname(__file__))) import config from scenario_loader import load_scenario, list_scenarios from scripts.data_to_mcap import convert_folder +from scripts.generate_shenron import process_session as generate_shenron_data +from pathlib import Path # ----------------------------------------------------------------------- @@ -281,8 +283,13 @@ def main(): ego_vehicle.destroy() if recorder: recorder.close() - # AUTO-MCAP Step + if os.path.exists(recorder.base_path): + # NEW: AUTO-SHENRON Step + print(f"[AUTO-SHENRON] Synthesizing physics-based radar for: {recorder.base_path}") + generate_shenron_data(Path(recorder.base_path)) + + # AUTO-MCAP Step print(f"[AUTO-MCAP] Triggering seamless conversion for: {recorder.base_path}") convert_folder(recorder.base_path)