diff --git a/.gitignore b/.gitignore index 43798a2..c64a723 100644 --- a/.gitignore +++ b/.gitignore @@ -4,4 +4,6 @@ __pycache__/ logs/ *.pyc .env -carla_examples/nvidia/ \ No newline at end of file +carla_examples/nvidia/ +1432_EVM/ +shenron_debug/ \ No newline at end of file diff --git a/config.py b/config.py index 0cce269..f0731cb 100644 --- a/config.py +++ b/config.py @@ -12,10 +12,10 @@ RADAR_HORIZONTAL_FOV = 120 RADAR_VERTICAL_FOV = 30 RADAR_RANGE = 100 -# Lidar settings (balanced for performance + quality) +# Lidar settings (HIGH FIDELITY for Shenron Radar Simulation) LIDAR_RANGE = 100 -LIDAR_CHANNELS = 32 -LIDAR_POINTS_PER_SECOND = 100000 +LIDAR_CHANNELS = 64 +LIDAR_POINTS_PER_SECOND = 1200000 LIDAR_ROTATION_FREQUENCY = FPS # Output diff --git a/intel/Shenron_Debug_Plan.md b/intel/Shenron_Debug_Plan.md new file mode 100644 index 0000000..7eebd50 --- /dev/null +++ b/intel/Shenron_Debug_Plan.md @@ -0,0 +1,62 @@ +# Shenron Physics-Based Radar: Debug & Troubleshooting Plan + +This document serves as a comprehensive guide for diagnosing and resolving the data-fidelity issues in the C-SHENRON radar integration. While the pipeline is functional, the resulting point clouds currently do not accurately reflect the scene geometry or velocity profiles. + +## 🕵️ 1. Immediate Red Flags (Potential Root Causes) + +Through architectural review, the following critical issues have been identified: + +### ⚡ A. The "Zero Velocity" Bug +In the current implementation of `lidar.py` and `generate_shenron.py`, the **Doppler/Velocity channel is currently hardcoded to 0**. +- **Found in:** `lidar.py:L164-165` (commented out) and `L209` (`rt_speed = np.zeros_like(rt_rho)`). +- **Impact:** The radar model receives no radial velocity information, resulting in static-only heatmaps and incorrect detector peaks. +- **Fix Required:** Extract the `velocity` of hit actors in CARLA and pass it into the `points[:, 3]` channel before synthesis. + +### 📐 B. Semantic LiDAR Column Mismatch +The bit-casting logic assumes a specific 6-column layout: `[x, y, z, cos, obj, tag]`. +- **The Issue:** CARLA 0.9.16 `ray_cast_semantic` raw data is often: + - `[0:x, 1:y, 2:z, 3:CosInclinationAngle, 4:ObjectIndex, 5:Tag]` +- **Found in:** `lidar.py:L149-168`. If we are reading `ObjectIndex` as `Cosines`, the reflectivity (loss) calculation will be garbage. +- **Fix Required:** Verify column indices in `src/recorder.py` and ensure `lidar.py` maps them correctly. + +### 🔄 C. Coordinate Transformation Logic +In `lidar.py:L156`, indices are swapped: `points[:, [0, 1, 2]] = test[:, [1, 0, 2]]`. +- **The Risk:** This transformation (X->Y, Y->X) might not align with the `theta` (azimuth) calculation or the internal coordinate system of the `Sceneset` occlusion removal logic. +- **Check:** Compare CARLA's forward vector (X) with Shenron's expected forward vector (often Y in legacy codebases). + +--- + +## 🛠️ 2. Step-by-Step Debugging Checklist + +### Phase 1: Data Integrity (LiDAR to Input) +- [ ] **Visualize Raw Semantic LiDAR**: Use a script to plot the Saved `.npy` files from the `lidar/` folder. Ensure the "Materials" (Tag field) are distinct (Vehicles vs Road vs Pedestrians). +- [ ] **Verify Bit-Casting**: Run `scripts/verify_tags.py`. Check if the unique tags recovered match the scene (e.g., if there's a car, Tag 10 must exist). + +### Phase 2: Radar Hardare Sync +- [ ] **Check Config.yaml**: Inspect `scripts/ISOLATE/sim_radar_utils/config.yaml`. Are the `samp_rate`, `chirpT`, and `gain` realistic for an ADAS radar (e.g., 77GHz)? +- [ ] **Validate `model_wrapper.py`**: Ensure the `_sync_configs` method correctly updates the underlying `radar` object. + +### Phase 3: Physics Engine (ISOLATE/shenron) +- [ ] **Occlusion Check**: In `Sceneset.py`, check the `removeocclusion_hiddenpoint` method. Is it being too aggressive and deleting the ego-vehicle's own target (the car in front)? +- [ ] **Phase Summation**: In `heatmap_gen_fast.py`, ensure the complex exponentiation `torch.exp(1j * ...)` preserves the phase shift related to target distance (`rho`) and velocity (`speed`). + +--- + +## 🛰️ 3. Relevant Files to Investigate + +1. **`scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/lidar.py`**: The bridge between recorded data and the physics model. **(High Priority)** +2. **`scripts/generate_shenron.py`**: The parallel orchestrator. **(Medium Priority)** +3. **`scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/shenron/heatmap_gen_fast.py`**: The core signal generator. **(High Priority)** +4. **`scripts/ISOLATE/model_wrapper.py`**: The API layer and config sync unit. **(Medium Priority)** + +--- + +## 💡 4. Context for the Next Session + +We have successfully built a pipeline that: +1. Records Semantic LiDAR from CARLA. +2. Bit-casts raw bytes into material tags. +3. Feed those tags into a physics-based radar simulator. +4. Serializes the results into MCAP (Rich PCD: `X, Y, Z, V, Mag`). + +**The Goal of the next session is to find why the "Rich" data doesn't look like the actual scene and fix the Doppler/Visibility fidelity.** diff --git a/intel/Shenron_debug.md b/intel/Shenron_debug.md new file mode 100644 index 0000000..b17ccd1 --- /dev/null +++ b/intel/Shenron_debug.md @@ -0,0 +1,180 @@ +# Shenron Physics Debug Log +**Date:** 2026-04-03 +**Engineer:** Fox ADAS Pipeline | Antigravity AI +**Objective:** Resolve physics-based discrepancies in the C-Shenron synthetic radar pipeline, align with AWRL1432BOOST hardware performance, and establish a stable, high-fidelity simulation baseline. + +--- + +## 📐 Architecture Overview + +The C-Shenron pipeline is a **physics-based FMCW radar simulator** isolated in `scripts/ISOLATE/`. It takes CARLA Semantic LiDAR as input and produces a synthetic radar point cloud as output. + +``` +CARLA (Semantic LiDAR) + → lidar.py (Ingestion + Material Mapping) + → Sceneset.py (Fresnel RCS Physics) + → heatmap_gen_fast.py (GPU FMCW ADC Synthesis) + → radar_processor.py (Range/Doppler FFT + CFAR) + → [x, y, z, velocity, magnitude] PointCloud +``` + +**Key Coordinate Convention (LOCKED):** +- CARLA Frame: `X = Forward`, `Y = Right (LHS)`, `Z = Up` +- Shenron Internal Frame: **`Index 1 = Forward`**, `Index 0 = Side` +- The swap `points[:, 0] = CARLA_Y, points[:, 1] = CARLA_X` in `lidar.py` is **intentional and must not be removed.** +- MCAP/Foxglove Output: Negate Y to convert from CARLA LHS → ROS RHS. + +--- + +## 🐛 Bugs Fixed + +### Bug #1: The "Cos(Cos)" Reflection Error +**File:** `scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/shenron/Sceneset.py` +**Root Cause:** The `specularpoints()` method received pre-computed cosine values from CARLA's Semantic LiDAR (`cos_inc_angle`), but then ran `np.cos()` on them again inside `get_loss_3()`. This caused a "double-cosine" effect. +**Effect:** A perfect perpendicular bounce (true `cos = 1.0`) was calculated as `cos(1.0) = 0.54`, implying a 57-degree glancing angle. This caused a massive ~46% RCS energy drop on every flat surface (car hoods, road, walls). +**Fix:** Removed the erroneous `np.cos()` call. The engine now directly uses the pre-computed cosine values as intended. Also corrected the specular reflection mask threshold for 2-degree incident angles. + +--- + +### Bug #2: Zeroed-Out Thermal Noise Floor +**File:** `scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/ConfigureRadar.py` +**Root Cause:** The noise generator was multiplied by zero: `signal_Noisy = 0 * (...)`. +**Effect:** With a zero noise floor, the CFAR detector became violently unstable. It detected infinite false peaks in empty bins because the background noise variance was literally zero, producing random "sparkle" detections with no correlation to actual targets. +**Fix:** Restored the AWGN noise generator with a proper complex noise implementation using `noise_amp * (np.random.randn(...) + 1j * np.random.randn(...))`. + +--- + +### Bug #3: LiDAR Metadata Bit-View Error +**File:** `src/recorder.py` +**Root Cause:** CARLA Semantic LiDAR stores `object_idx` and `semantic_tag` as `uint32` values packed into `float32` bit fields. The recorder was reading them as plain floats, producing garbage values (e.g., Object ID of `0.0` for every actor). +**Effect:** The velocity projection calculation `v_radial = dot(actor_vel - ego_vel, direction)` always failed the actor lookup, resulting in zero radial velocity stored for every point. +**Fix:** Applied `np.view(np.uint32)` to correctly unpack the integer IDs from the float32 bitstream. The same fix was propagated to the MCAP packaging scripts (`test_shenron.py`, `data_to_mcap.py`). + +--- + +### Bug #4: Stale `RadarProcessor` Axes (Range Scaling Mismatch) +**File:** `scripts/ISOLATE/model_wrapper.py` +**Root Cause:** The `RadarProcessor` only computed its range axis once at startup, using the default `config.yaml` values (24GHz, 250MHz). When processing the `awrl1432` profile (77GHz, 400MHz), the processor still used the old stale axes to convert FFT bins to meters. +**Effect:** A ~2x range "stretching" — objects at 50m appeared at ~100m. The bandwidth mismatch caused incorrect scaling. +**Fix:** Added `self.processor.__init__()` call inside `_sync_configs()` to force the processor to rebuild its internal range and velocity axes every time the hardware profile changes. + +--- + +## 📡 Hardware Profile: AWRL1432 (Iteration 07 — Current Best) + +The `awrl1432` profile in `ConfigureRadar.py` has been tuned to match the real-world **TI AWRL1432BOOST** professional ADAS configuration: + +| Parameter | Previous (Iter 05) | Current (Iter 07) | Notes | +| :--- | :--- | :--- | :--- | +| **Frequency** | 77 GHz | 77 GHz | Same | +| **Bandwidth (B)** | 137.2 MHz | **400 MHz** | 3x wider | +| **Range Resolution** | ~109 cm | **37.5 cm** | 3x sharper | +| **Max Range** | ~279 m | **~96 m** | Calibrated to ADAS zone | +| **Chirps (Np)** | 128 | **64** | 2x faster, maintained SNR | +| **Samples (N)** | 256 | 256 | Unchanged | +| **Antennas (nRx)** | 6 | 6 | Unchanged | + +**Signal Processor Config** (`sim_radar_utils/config.yaml`): +- `fStrt: 77.0e9`, `fStop: 77.4e9` (matching 400MHz bandwidth) +- `Np: 64` (matching hardware chirp count) +- `NFFT: 256` (matching N_sample) + +--- + +## 📦 Data Pipeline Fixes + +### Sensor Mount Calibration (MCAP Visualization) +Both `scripts/test_shenron.py` and `scripts/data_to_mcap.py` now include correct physical mount offsets so sensors align properly in Foxglove: +- **LiDAR:** `Z = +2.5m` (Roof mount) +- **Radar (Native + Shenron):** `X = +2.0m, Z = +1.0m` (Front bumper/grille) + +### LiDAR Point Cloud — Full 7-Column Support +Both MCAP scripts now publish all 7 Semantic LiDAR fields: +``` +[x, y, z, velocity, cos_inc_angle, object_id, semantic_tag] +``` +`object_id` and `semantic_tag` are correctly decoded via `.view(np.uint32)` before packaging. + +--- + +## 🏃 Performance Optimization + +**Testbench Speed (for 9 seconds of data):** +| State | Time | Cause | +| :--- | :--- | :--- | +| Before Today | ~20 min | 128 chirps × 3 models | +| Iteration 06 | ~5 min | 32 chirps × 3 models | +| **Iteration 07 (Current)** | **~5-7 min** | **64 chirps × 1 model (awrl1432 only)** | + +To re-enable all three radar models, update `test_shenron.py` line 84: +```python +radar_types = ['awrl1432', 'radarbook', 'ti_cascade'] +``` + +--- + +## 🧭 Coordinate System (Final Reference — DO NOT CHANGE) + +This is the authoritative mapping that must be maintained across all files: + +``` +CARLA LiDAR Output: + col[0] = X (Forward) + col[1] = Y (Right, LHS) + col[2] = Z (Up) + +Shenron Internal Input (after lidar.py swap): + col[0] = CARLA Y (Side) <-- Index 0 is Side + col[1] = CARLA X (Forward) <-- Index 1 is Forward + col[2] = CARLA Z (Up) + +Shenron Cropping Filter (Cropped_forRadar): + skew_pc[:, 1] > 0.5 <-- Forward filter (Y=Index 1) + skew_pc[:, 0] in [-30, +30] <-- Side filter (X=Index 0) + skew_pc[:, 1] < 120 <-- Max forward range + +MCAP Output (ROS/Foxglove RHS): + Negate Y column before packaging + Apply sensor mount pose offsets +``` + +--- + +## ▶️ Running Future Iterations + +```powershell +# Activate Environment +conda activate carla312 + +# Run the testbench (replace XXXX with your iteration name) +python scripts/test_shenron.py --iter "07_high_def_sync" + +# Output: Shenron_debug/iterations/07_high_def_sync/07_high_def_sync.mcap +``` + +### Known Pending Issues (Next Steps) +1. **Turn Lag:** Shenron points appear to trail ~0.5-1 frame behind the CARLA native radar during sharp turns. Suspected cause: LiDAR data captured at `T-1` but rendered with ego pose at `T`. Requires timestamp sync investigation in `recorder.py`. +2. **Angular FOV Validation:** Compare Shenron angular output vs. AWRL1432BOOST hardware spec (`+/- 60°`) to ensure angular clipping is not removing valid detections. +3. **CFAR Threshold Tuning:** The `threshold: 20` in `config.yaml` may need adjustment after the noise floor restoration. Consider running a "Clear Road" baseline to calibrate the false alarm rate. + +--- + +## 📁 Key Files Reference + +| File | Role | +| :--- | :--- | +| `scripts/ISOLATE/model_wrapper.py` | Public API — single entry point for Shenron | +| `scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/lidar.py` | LiDAR ingestion, semantic mapping, axis swap | +| `scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/ConfigureRadar.py` | Hardware profiles (awrl1432, radarbook, ti_cascade) | +| `scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/shenron/Sceneset.py` | Fresnel reflection + RCS physics | +| `scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/shenron/heatmap_gen_fast.py` | GPU FMCW ADC synthesis | +| `scripts/ISOLATE/sim_radar_utils/radar_processor.py` | Range/Doppler FFT + CFAR detection | +| `scripts/ISOLATE/sim_radar_utils/config.yaml` | DSP processor configuration | +| `scripts/test_shenron.py` | Testbench — generates and packages iterations | +| `scripts/data_to_mcap.py` | Main MCAP converter (Dashboard path) | +| `src/recorder.py` | Data capture — includes velocity + semantic metadata | +| `intel/shenron_architecture_deepdive.html` | Visual HTML architecture guide | + +--- + +*Generated by Antigravity AI | Fox CARLA ADAS Pipeline | 2026-04-03* diff --git a/intel/shenron_architecture_deepdive.html b/intel/shenron_architecture_deepdive.html new file mode 100644 index 0000000..1bf61a7 --- /dev/null +++ b/intel/shenron_architecture_deepdive.html @@ -0,0 +1,352 @@ + + + + + + C-Shenron Physics Architecture Deep-Dive + + + + + + + +
+
+
Radar Physics Engine v4.2
+

C-Shenron Deep Architecture

+

From Raw LiDAR Scents to Synthetic FMCW Realities

+
+ + + + + + + + LiDAR + + + RCS + + + ADC + + + PCD + + + + + + + + + + + + + +
+
+
01
+
Scene Ingestion
+

Converts raw Semantic LiDAR into physical material properties. Vehicles become High-Permittivity Metal, Foliage becomes Diffuse Carbon.

+
map_carla_semantic_lidar_latest()
→ Recovered Tags via .view(np.uint32)
+
+ +
+
02
+
Physics Modeling
+

Calculates the Fresnel Reflection loss based on incident angles. Handles Specular (mirror) vs Diffuse (scattering) coefficients.

+
Sceneset.specularpoints()
Pr = (Pt * G² * λ² * σ) / ((4π)³ * R⁴)
+
+ +
+
03
+
FMCW Synthesis
+

Synthesizes raw Analog-to-Digital samples. Models the Beat Frequency ($f_{beat}$) across multiple Rx antennas & chirps.

+
heatmap_gen_fast.py
signal = exp(j * 2π * f_beat * t)
+
+ +
+
04
+
DSP Pipeline
+

Processes the raw signal through Range/Doppler FFTs and CA-CFAR Detection to generate the final synthetic PointCloud.

+
RadarProcessor.cal_doppler_fft()
N=256, Np=128 (Configurable)
+
+
+ +
+

⚡ Today's Optimization Discoveries

+

Summary of the hardware-to-model alignment fixes applied in Iteration 05.

+ +
+
+

Detected Failure

+

The 'Cos(Cos)' Bug in Sceneset.py. The engine was calculating np.cos(angles) on values that were already cosines. Result: 46% energy loss on every bounce.

+
+
+

Engineering Fix

+

Removed the double-dampening logic. Physics engine now correctly interprets incident angles from CARLA, restoring realistic metallic RCS signatures.

+
+ +
+

Detected Failure

+

The 'Stationary Reality' Bug. Moving targets appeared stationary or dragged behind the car. Result: Radial velocity was being zeroed out due to incorrect bit-interpretations.

+
+
+

Engineering Fix

+

Synchronized World-Pose and LiDAR-Tick via recorder.py update. High-fidelity Doppler detection now tracks dynamic NPC movement at 64Hz.

+
+
+
+ +

🛠️ Hardware Support Matrix

+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Radar ModelFrequencyChirps (Np)Samples (N)Logic Layer
AWRL1432 BOOST77-81 GHz128256Rich Physics
TI Cascade Array77-81 GHz3 (Fast)256Phase-Prime
Generic Radarbook24 GHz128256Standard
+
+ + +
+ + + diff --git a/intel/shenron_pipeline_analysis.md b/intel/shenron_pipeline_analysis.md new file mode 100644 index 0000000..31a81cc --- /dev/null +++ b/intel/shenron_pipeline_analysis.md @@ -0,0 +1,62 @@ +# Shenron Physics Pipeline Deep-Dive + +To understand why the simulated data isn't matching your AWRL1432BOOST real-world recordings, we need to look under the hood of Shenron's physics engine. The pipeline translates native CARLA Semantic LiDAR into raw radio frequency (RF) samples before they are ever clustered into points. + +Here is the step-by-step technical data flow and where the physics mappings occur: + +## 1. Scene Geometry & Material Ingestion (`lidar.py`) +CARLA native radar is sparsely ray-cast. To achieve high-fidelity FMCW simulation, Shenron uses **Semantic LiDAR** as a dense scene representation. +* It ingests the point cloud shape: `[x, y, z, radial_velocity, cos_inc_angle, object_idx, semantic_tag]`. +* Using `new_map_material()`, it re-maps CARLA's `semantic_tag` into electromagnetic properties. For example, Vehicles become "Metal" ($R_{perm} = 100000$), Buildings become "Concrete" ($R_{perm} = 5.24$), and Pedestrians remain low-reflectivity models. + +## 2. Electromagnetics & RCS Modeling (`Sceneset.py`) +This is where the standard geometric points are converted into Radar Cross Sections (RCS) or "Reflection Powers". +* The engine converts Cartesian coordinates `(x, y, z)` into Spherical coordinates `(rho, theta, elevation)`. +* It calls `get_loss_3()` to calculate **Signal Attenuation** based on the Radar Equation $\frac{P_r}{P_t} = \frac{G^2 \sigma \lambda^2}{(4\pi)^3 R^4}$. +* The total loss is split into two physical properties: + 1. **Specular Reflection ($P_{spec}$):** Direct mirror-like bounces. Extremely high power but requires the surface normal to be almost perfectly aimed at the sensor ($\theta < 2^\circ$). + 2. **Diffuse Scattering ($P_{scat}$):** Lambertian scattering (rough surfaces like roads and grass that scatter waves in all directions). +* It mathematically uses Fresnel Coefficients (permittivity) and surface roughness equations to calculate the final target amplitude. + +## 3. ADC Raw Signal Generation (`heatmap_gen_fast.py` / `CUDA`) +Once Shenron knows exactly *where* the points are and *how loud* they should reflect, it builds the actual Analog-to-Digital Converter (ADC) data cube (`[Chirps, Antennas, Samples]`). +* **Time of Flight ($\tau$):** Uses $\tau = \frac{2R}{c} + \frac{2v \cdot T_{rep}}{c}$ to model range scaling and the Doppler frequency shift across successive chirps based on the `radial_velocity`. +* **Antenna Phase ($\Delta$):** Calculates the slight delays between different Rx antennas based on the `theta` (Azimuth) spacing. +* **Dechirping:** Multiplies the transmitted FMCW sweep by the returning simulated waves to generate the intermediate frequency (IF) `signal`. +* **Thermal Noise:** Generates the raw ADC noise floor via `np.random.normal()` and adds it to the clean signal. + +## 4. Signal Processing (`RadarProcessor`) +Finally, `model_wrapper.py` passes the synthetic `adc_data` to a traditional DSP pipeline: +* **Range FFT:** Resolves the objects into specific distance bins. +* **Doppler FFT:** Resolves object velocities. +* **CFAR Detection:** Identifies peaks above the surrounding noise floor. +* **Angle of Arrival:** Resolves Azimuth. + +--- + +# 🚨 Critical Bugs Identified + +While doing this trace, I discovered **three major structural flaws** in the Shenron code that explain exactly why your point clouds look erratic, static, or entirely uncorrelated to your physical AWRL1432: + +### Bug 1: The "Cos(Cos)" Reflection Bug +In `lidar.py` (Line 150 & 168), the system extracts the `cos_inc_angle` from CARLA (`sem_lidar_frame[:, 4]`) and assigns it to the index `angles_carla`. +However, inside `get_loss_3()` (Line 420), it takes this value and runs `np.cos(angles)`. +* **The Error:** By executing `cos(cos_value)`, the math breaks completely. A perfect perpendicular bounce (which has a true cosine of 1.0) is calculated as $cos(1.0) = 0.5403$ (which implies a massive 57-degree glancing deflection!). +* **The Result:** The radar physics engine fundamentally miscalculates the physical reflection, causing giant RCS drops on flat surfaces like car chassis, wiping out the specular points you expect to see. + +### Bug 2: Zeroed-Out Thermal Noise Floor +In `ConfigureRadar.py` (Line 133) for both `radarbook` and `awrl1432`, the noise generator code looks like this: +```python +signal_Noisy = np.random.normal(0,1,size=(self.nRx,self.N_sample)) +signal_Noisy = 0*(signal_Noisy + 1j*signal_Noisy) +``` +* **The Error:** The entire noise floor is forcefully zeroed out (`0 * ...`). +* **The Result:** Without a thermal noise floor, the Constant False Alarm Rate (CFAR) algorithm in `RadarProcessor` becomes violently unstable. It starts detecting infinite peaks in random empty bins because the background noise variance is literally zero, causing "random values rather than actual vehicle movements." + +### Bug 3: Hardcoded Backwards Override +In `generate_shenron.py` (Line 38), the orchestrator explicitly ignores our shiny new config file: Let's look at the initialization: +```python +model = ShenronRadarModel(radar_type='radarbook') +``` +* **The Error:** The batch processing script is strictly hardcoded to the old `radarbook` profile. +* **The Result:** Even though we set `awrl1432` as the default in `simulator_configs.yaml`, the offline generation script completely ignores it, so you're still processing data at 24GHz with 1MHz sampling! diff --git a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/ConfigureRadar.py b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/ConfigureRadar.py index 7c96537..5665543 100644 --- a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/ConfigureRadar.py +++ b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/ConfigureRadar.py @@ -78,6 +78,39 @@ class radar(): self.voxel_phi = 2 # 0.5 # 0.1 self.voxel_rho = 0.05 # 0.1 # 0.05 + elif radartype == "awrl1432": + self.center = np.array([0.0, 4.0]) # center of radar + self.elv = np.array([0.5]) # self position in z-axis + self.orientation = 90 # orientation of radar + + self.f = 77e9 + self.B = 400e6 # 400MHz (37.5cm resolution) + self.c = 3e8 + self.N_sample = 256 + self.samp_rate = 12.5e6 + self.doppler_mode = 1 + self.chirps = 64 # Optimized: 2x faster, but with high SNR for Angular precision + self.nRx = 6 + self.noise_amp = 0.005 + self.gain = 10 ** (105 / 10) + self.angle_fft_size = 256 + + self.range_res = self.c / (2 * self.B) + self.max_range = self.range_res * self.N_sample + + self.chirpT = self.N_sample / self.samp_rate ## Time of chirp + self.chirp_rep = 36.4e-6 + self.idle = self.chirp_rep - self.chirpT ## Idle time + + Ts = 1 / self.samp_rate + self.t = np.arange(0, self.chirpT, Ts) + self.tau_resolution = 1 / self.B + self.k = self.B / self.chirpT + + self.voxel_theta = 2.0 # 0.5 # 0.1 + self.voxel_phi = 2.0 # 0.5 # 0.1 + self.voxel_rho = 0.05 # 0.1 # 0.05 + else: raise Exception("Incorrect radartype selected") @@ -93,11 +126,11 @@ class radar(): # for low resolution 16 channels signal_Noisy = np.random.normal(0,1,size=(self.nRx,self.N_sample)) - signal_Noisy = 0*(signal_Noisy + 1j*signal_Noisy) + signal_Noisy = (signal_Noisy + 1j*np.random.normal(0,1,size=(self.nRx,self.N_sample))) * self.noise_amp - elif self.radartype == "radarbook": + elif self.radartype == "radarbook" or self.radartype == "awrl1432": signal_Noisy = np.random.normal(0,1,size=(self.nRx,self.N_sample)) - signal_Noisy = 0*(signal_Noisy + 1j*signal_Noisy) + signal_Noisy = (signal_Noisy + 1j*np.random.normal(0,1,size=(self.nRx,self.N_sample))) * self.noise_amp else: raise Exception("Incorrect radartype selected") diff --git a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/lidar.py b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/lidar.py index 85c755e..ed54598 100644 --- a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/lidar.py +++ b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/lidar.py @@ -85,14 +85,15 @@ def Cropped_forRadar(pc, veh_coord, veh_angle, radarobj): # skew_pc = np.vstack(((skew_pc ).T, pc[:, 3], pc[:, 5])).T #x,y,z,speed,material skew_pc = np.vstack(((skew_pc ).T, pc[:, 3], pc[:, 5],pc[:,6])).T #x,y,z,speed,material, cosines - rowy = np.where((skew_pc[:, 1] > 0.8)) + # Restoration: X is Side (Index 0), Y is Forward (Index 1) + rowy = np.where((skew_pc[:, 1] > 0.5)) # Start 0.5m forward to avoid bumper new_pc = skew_pc[rowy, :].squeeze(0) - new_pc = new_pc[new_pc[:,4]!=0] + new_pc = new_pc[new_pc[:,4]!=0] # Drop empty materials - new_pc = new_pc[(new_pc[:,0]<50)*(new_pc[:,0]>-50)] - new_pc = new_pc[(new_pc[:,1]<100)] - new_pc = new_pc[(new_pc[:,2]<2)] + new_pc = new_pc[(new_pc[:,0]<30)*(new_pc[:,0]>-30)] # Side range (X) is +-30m + new_pc = new_pc[(new_pc[:,1]<120)] # Forward range (Y) extended to 120m for headroom + new_pc = new_pc[(new_pc[:,2]<10)] # Height cut-off simobj = Sceneset(new_pc) @@ -151,9 +152,12 @@ def run_lidar(sim_config, sem_lidar_frame): load_pc = map_carla_semantic_lidar_latest(load_pc) test = new_map_material(load_pc) + # LEGACY REQUIREMENT: Shenron physics engine expects Index 1 to be Forward. + # We swap CARLA's X (Forward) into Index 1 and CARLA's Y (Right) into Index 0. points = np.zeros((np.shape(test)[0], 7)) - # points[:, [0, 1, 2]] = test[:, [0, 2, 1]] - points[:, [0, 1, 2]] = test[:, [1, 0, 2]] + points[:, 0] = test[:, 1] # Side (Y in CARLA) + points[:, 1] = test[:, 0] # Forward (X in CARLA) + points[:, 2] = test[:, 2] # Up (Z in CARLA) """ points mapping @@ -161,8 +165,8 @@ def run_lidar(sim_config, sem_lidar_frame): +ve ind 1 == +ve depth +ve ind 2 == +ve height """ - # add the velocity channel here to the lidar points on the channel number 3 most probably - # points[:, 3] = load_velocity + # Embed the velocity channel dynamically passed from CARLA Semantic LiDAR + points[:, 3] = sem_lidar_frame[:, 3] points[:, 5] = test[:, 3] points[:, 6] = cosines diff --git a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/shenron/Sceneset.py b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/shenron/Sceneset.py index f8d80e2..399e853 100644 --- a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/shenron/Sceneset.py +++ b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/shenron/Sceneset.py @@ -415,24 +415,27 @@ def get_loss_3(points, rho, elev_angle, angles, radar, use_spec = True, use_diff material = np.asarray(material, dtype = 'int') # material[:] = 3 + # BUG FIX: CARLA Semantic LiDAR passes the actual cosine value for 'angles', + # not the angle in radians. Calling np.cos(angles) was creating cos(cos_theta). + cos_inc = np.abs(angles) + sin_inc_sq = 1.0 - np.power(cos_inc, 2) # sin^2 + cos^2 = 1 + #Material dependent qualtities # if use_spec and use_diffused: - R_TE_sq = np.power(((abs(np.cos(angles)) - np.sqrt(permittivity[material] - np.power(np.sin(angles),2)))/(abs(np.cos(angles)) + np.sqrt(permittivity[material] - np.power(np.sin(angles),2)))),2) - R_TM_sq = np.power(((permittivity[material]*abs(np.cos(angles)) - np.sqrt(permittivity[material] - np.power(np.sin(angles),2)))/(permittivity[material]*abs(np.cos(angles)) + np.sqrt(permittivity[material] - np.power(np.sin(angles),2)))),2) + R_TE_sq = np.power(((cos_inc - np.sqrt(permittivity[material] - sin_inc_sq))/(cos_inc + np.sqrt(permittivity[material] - sin_inc_sq))),2) + R_TM_sq = np.power(((permittivity[material]*cos_inc - np.sqrt(permittivity[material] - sin_inc_sq))/(permittivity[material]*cos_inc + np.sqrt(permittivity[material] - sin_inc_sq))),2) R_sq = R_TE_sq #0.5 * (R_TE_sq + R_TM_sq) - rough_sq = np.exp(-0.5*np.power(4*np.pi*roughness[material]*abs(np.cos(angles))*radar.f/radar.c,2)) - # if len(rough_sq[abs(angles) < spec_angle_thresh])!=0: - # print(f"Materials = {np.unique(material)}") - # print(f"R value max= {np.max(rough_sq[abs(angles) < spec_angle_thresh])}; average = {np.mean(rough_sq[abs(angles) < spec_angle_thresh])}") + rough_sq = np.exp(-0.5*np.power(4*np.pi*roughness[material]*cos_inc*radar.f/radar.c,2)) S_sq = 1 - rough_sq # Scattering coefficient = P_scat/P_reflec P_reflected = P_incident * R_sq P_scat = P_reflected * S_sq - P_scat_lobe = scat_normalization*(lobe_frac * np.power(abs(np.cos(angles)),2*spec_lobe_exp) + (1-lobe_frac)) + P_scat_lobe = scat_normalization*(lobe_frac * np.power(cos_inc, 2*spec_lobe_exp) + (1-lobe_frac)) P_scat = P_scat * np.power(P_scat_lobe,2) #lobe shape is for amplitude, need to square for power P_spec = P_reflected * rough_sq - P_spec = P_spec * np.ones((len(angles),)) * (abs(angles) < spec_angle_thresh) # specular reflection only if angle < 2 degrees + # Use the cosine threshold for 2 degrees (0.99939). cos(inc_angle) > cos(2deg) means incident angle < 2deg + P_spec = P_spec * np.ones((len(angles),)) * (cos_inc > np.cos(spec_angle_thresh)) if not use_spec: # R_TE_sq = np.power(((abs(np.cos(angles)) - np.sqrt(permittivity[material] - np.power(np.sin(angles),2)))/(abs(np.cos(angles)) + np.sqrt(permittivity[material] - np.power(np.sin(angles),2)))),2) diff --git a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/shenron/heatmap_gen_fast.py b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/shenron/heatmap_gen_fast.py index eac4e02..d746162 100644 --- a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/shenron/heatmap_gen_fast.py +++ b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/shenron/heatmap_gen_fast.py @@ -61,9 +61,11 @@ def heatmap_gen(rho, theta, loss, speed, radar, plot_fig, return_power): gpu_id, _ = get_gpu_id_most_avlbl_mem() device = torch.device(f'cuda:{gpu_id}') else: + print("[SHENRON DEBUG] torch.cuda.is_available() returned False. Check PyTorch installation.") gpu_id = 0 device = torch.device('cpu') - except Exception: + except Exception as e: + print(f"[SHENRON DEBUG] Exception during CUDA setup: {e}") gpu_id = 0 device = torch.device('cpu') diff --git a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/simulator_configs.yaml b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/simulator_configs.yaml index f20f12f..c59327e 100644 --- a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/simulator_configs.yaml +++ b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/simulator_configs.yaml @@ -4,8 +4,8 @@ NOISE : True BUILDINGS : True -# RADAR_TYPE : ti_cascade # 1. radarbook | 2. ti_cascade -RADAR_TYPE : ti_cascade +# RADAR_TYPE : awrl1432 # 1. radarbook | 2. ti_cascade | 3. awrl1432 +RADAR_TYPE : awrl1432 INDOOR : False diff --git a/scripts/ISOLATE/model_wrapper.py b/scripts/ISOLATE/model_wrapper.py index 2d20bfd..2106338 100644 --- a/scripts/ISOLATE/model_wrapper.py +++ b/scripts/ISOLATE/model_wrapper.py @@ -67,6 +67,10 @@ class ShenronRadarModel: ur.fftCfg['NFFTVel'] = self.radar_obj.chirps print(f"Synced global config: N={ur.radarCfg['N']}, Np={ur.radarCfg['Np']}, Ant={ur.radarCfg['NrChn']}") + + # CRITICAL: Re-initialize the internal axes of the processor to match new hardware + if hasattr(self, 'processor'): + self.processor.__init__() def process(self, semantic_lidar_data): """ @@ -84,6 +88,9 @@ class ShenronRadarModel: return np.empty((0, 5)) try: + # Re-sync global configs for this specific model, in case another model overwrote them + self._sync_configs() + # 1. Physics-based Signal Generation (FMCW Chirps) # This generates the raw ADC samples [Np, N, Ant] adc_data = run_lidar(self.sim_config, semantic_lidar_data) diff --git a/scripts/ISOLATE/sim_radar_utils/config.yaml b/scripts/ISOLATE/sim_radar_utils/config.yaml index 564c8ac..a60e8e3 100644 --- a/scripts/ISOLATE/sim_radar_utils/config.yaml +++ b/scripts/ISOLATE/sim_radar_utils/config.yaml @@ -1,15 +1,14 @@ radar: - fStrt: 24.0e9 # start frequency - fStop: 24.25e9 # stop frequency - TRampUp: 100e-6 # ramp-up time of chirp - TInt: 100e-3 # inter-frame interval - Tp: 1000e-6 # inter-chirp time - N: 256 # samples per chirp - IniTim: 100e-3 # initial delay before collection of samples - IniEve: 0 # start automatically after IniTim -# TxSeq: [1, 2] - TxSeq: [1] # antenna activation sequence - Np: 3 #128 # number of chirps in one frame + fStrt: 77.0e9 # GHz + fStop: 77.4e9 # 400 MHz Bandwidth + TRampUp: 100e-6 + TInt: 100e-3 + Tp: 1000e-6 + N: 256 + IniTim: 100e-3 + IniEve: 0 + TxSeq: [1] + Np: 64 # Optimized balance of Speed and SNR AntIdx: [0, 1, 2, 3, 4, 5, 6, 7, 9, 10, 11, 12, 13, 14, 15] # AntIdx: [0, 1, 2, 3, 4, 5, 6, 7] NrChn: 86 #16 diff --git a/scripts/data_to_mcap.py b/scripts/data_to_mcap.py index 7951086..289cf07 100644 --- a/scripts/data_to_mcap.py +++ b/scripts/data_to_mcap.py @@ -149,13 +149,39 @@ def convert_folder(folder_path): lidar_path = os.path.join(folder_path, "lidar", frame["lidar"]) if os.path.exists(lidar_path): points = np.load(lidar_path) - ros_points = points[:, :3].copy() - ros_points[:, 1] = -ros_points[:, 1] + # Robustness handle 6 vs 7 cols + if points.shape[1] == 6: + # Pad to [x, y, z, velocity, cos, obj, tag] + padded = np.zeros((points.shape[0], 7), dtype=np.float32) + padded[:, 0:3] = points[:, 0:3] + padded[:, 4] = points[:, 3] # cos + padded[:, 5] = points[:, 4].view(np.uint32).astype(np.float32) # obj + padded[:, 6] = points[:, 5].view(np.uint32).astype(np.float32) # tag + ros_points = padded + else: + ros_points = points.copy().astype(np.float32) + # Correct bits for [x,y,z,vel,cos,obj,tag] + ros_points[:, 5] = ros_points[:, 5].view(np.uint32).astype(np.float32) + ros_points[:, 6] = ros_points[:, 6].view(np.uint32).astype(np.float32) + + ros_points[:, 1] = -ros_points[:, 1] # RHS conversion + + # MOUNT OFFSET: LiDAR is on the roof (Z=2.5) + lidar_pose = {"position": {"x": 0.0, "y": 0.0, "z": 2.5}, "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}} + lidar_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(ros_points.astype(np.float32).tobytes()).decode("ascii") + "frame_id": "ego_vehicle", "pose": lidar_pose, "point_stride": 28, + "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}, + {"name":"cos_inc_angle","offset":16,"type":7}, + {"name":"object_id","offset":20,"type":7}, + {"name":"semantic_tag","offset":24,"type":7} + ], + "data": base64.b64encode(ros_points.tobytes()).decode("ascii") } writer.add_message(lidar_channel_id, log_time=ts_ns, data=json.dumps(lidar_msg).encode(), publish_time=ts_ns) @@ -175,10 +201,13 @@ def convert_folder(folder_path): # Stack X, Y, Z, and Velocity (4 floats = 16 bytes stride) radar_points = np.stack([xr, yr, zr, vel], axis=1).astype(np.float32) + # MOUNT OFFSET: Radar is on the bumper (X=2.0, Z=1.0) + radar_pose = {"position": {"x": 2.0, "y": 0.0, "z": 1.0}, "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}} + radar_msg = { "timestamp": {"sec": ts_sec, "nsec": ts_nsec}, "frame_id": "ego_vehicle", - "pose": identity_pose, + "pose": radar_pose, "point_stride": 16, "fields": [ {"name":"x","offset":0,"type":7}, @@ -202,10 +231,13 @@ def convert_folder(folder_path): ros_shenron = s_data.copy().astype(np.float32) ros_shenron[:, 1] = -ros_shenron[:, 1] # Negate Y for ROS + # MOUNT OFFSET: Shenron Radar is on the bumper (X=2.0, Z=1.0) + shenron_pose = {"position": {"x": 2.0, "y": 0.0, "z": 1.0}, "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}} + shenron_msg = { "timestamp": {"sec": ts_sec, "nsec": ts_nsec}, "frame_id": "ego_vehicle", - "pose": identity_pose, + "pose": shenron_pose, "point_stride": 20, # 5 floats * 4 bytes "fields": [ {"name":"x","offset":0,"type":7}, diff --git a/scripts/test_shenron.py b/scripts/test_shenron.py new file mode 100644 index 0000000..e71d208 --- /dev/null +++ b/scripts/test_shenron.py @@ -0,0 +1,270 @@ +import os +import sys +import numpy as np +import tqdm +from pathlib import Path +import json +import base64 +import argparse +from mcap.writer import Writer + +# Add project root and ISOLATE paths +project_root = Path(__file__).parent.parent +sys.path.append(str(project_root)) +sys.path.append(str(project_root / 'scripts' / 'ISOLATE')) + +try: + from scripts.ISOLATE.model_wrapper import ShenronRadarModel +except ImportError as e: + print(f"Error: Failed to import ShenronRadarModel. Ensure scripts/ISOLATE/model_wrapper.py exists. ({e})") + sys.exit(1) + +# Official Foxglove JSON Schemas +FOXGLOVE_POSE_SCHEMA = { + "$schema": "https://json-schema.org/draft/2020-12/schema", + "$id": "foxglove.Pose", + "title": "foxglove.Pose", + "type": "object", + "properties": { + "position": {"type": "object", "properties": {"x": {"type": "number"}, "y": {"type": "number"}, "z": {"type": "number"}}}, + "orientation": {"type": "object", "properties": {"x": {"type": "number"}, "y": {"type": "number"}, "z": {"type": "number"}, "w": {"type": "number"}}} + } +} +FOXGLOVE_IMAGE_SCHEMA = { + "$schema": "https://json-schema.org/draft/2020-12/schema", + "$id": "foxglove.CompressedImage", + "title": "foxglove.CompressedImage", + "type": "object", + "properties": { + "timestamp": {"type": "object", "properties": {"sec": {"type": "integer"}, "nsec": {"type": "integer"}}}, + "frame_id": {"type": "string"}, + "data": {"type": "string", "contentEncoding": "base64"}, + "format": {"type": "string"} + } +} +FOXGLOVE_PCL_SCHEMA = { + "$schema": "https://json-schema.org/draft/2020-12/schema", + "$id": "foxglove.PointCloud", + "title": "foxglove.PointCloud", + "type": "object", + "properties": { + "timestamp": {"type": "object", "properties": {"sec": {"type": "integer"}, "nsec": {"type": "integer"}}}, + "frame_id": {"type": "string"}, + "pose": FOXGLOVE_POSE_SCHEMA, + "point_stride": {"type": "integer"}, + "fields": { + "type": "array", + "items": {"type": "object", "properties": {"name": {"type": "string"}, "offset": {"type": "integer"}, "type": {"type": "integer"}}} + }, + "data": {"type": "string", "contentEncoding": "base64"} + } +} + +def load_frames(folder_path): + with open(os.path.join(folder_path, "frames.jsonl")) as f: + for line in f: + yield json.loads(line) + +def run_testbench(iter_name): + # Setup directories + debug_dir = project_root / 'Shenron_debug' + logs_dir = debug_dir / 'logs' + iter_dir = debug_dir / 'iterations' / iter_name + + if not logs_dir.exists(): + print(f"[ERROR] Required base folder {logs_dir} not found!") + return + + lidar_dir = logs_dir / 'lidar' + if not lidar_dir.exists(): + print(f"[ERROR] Required base folder {lidar_dir} not found!") + return + + iter_dir.mkdir(parents=True, exist_ok=True) + radar_types = ['awrl1432'] + + print(f"\n======================================") + print(f"SHENRON TESTBENCH ITERATION: {iter_name}") + print(f"======================================") + + # 1. GENERATE SYNTHETIC DATA + print("\n[Stage 1]: Processing Physics models...") + models = {} + for r_type in radar_types: + try: + print(f" -> Initializing {r_type} engine...") + models[r_type] = ShenronRadarModel(radar_type=r_type) + (iter_dir / r_type).mkdir(exist_ok=True) + except Exception as e: + print(f" -> [WARNING] Failed to init {r_type}: {e}") + + lidar_files = sorted(list(lidar_dir.glob("*.npy"))) + for lidar_file in tqdm.tqdm(lidar_files, desc=" Simulating Radars", unit="frame"): + data = np.load(lidar_file) + # Pad to [x, y, z, intensity, cos_inc_angle, obj, tag] if needed + if data.shape[1] == 6: + padded_data = np.zeros((data.shape[0], 7), dtype=np.float32) + padded_data[:, 0:3] = data[:, 0:3] + padded_data[:, 4:7] = data[:, 3:6] + data = padded_data + + for r_type, model in models.items(): + try: + rich_pcd = model.process(data) + out_path = iter_dir / r_type / lidar_file.name + np.save(out_path, rich_pcd) + except Exception as e: + print(f"[ERROR] Frame {lidar_file.name} failed for {r_type}: {e}") + + # 2. GENERATE MCAP + print("\n[Stage 2]: Weaving MCAP Comparison Package...") + output_mcap = iter_dir / f"{iter_name}.mcap" + + with open(output_mcap, "wb") as f: + writer = Writer(f) + writer.start(profile="foxglove") + + # Register Schemas + pose_schema_id = writer.register_schema(name="foxglove.Pose", encoding="jsonschema", data=json.dumps(FOXGLOVE_POSE_SCHEMA).encode()) + camera_schema_id = writer.register_schema(name="foxglove.CompressedImage", encoding="jsonschema", data=json.dumps(FOXGLOVE_IMAGE_SCHEMA).encode()) + lidar_schema_id = writer.register_schema(name="foxglove.PointCloud", encoding="jsonschema", data=json.dumps(FOXGLOVE_PCL_SCHEMA).encode()) + + # Register Channels + channels = { + 'camera': writer.register_channel(topic="/camera", message_encoding="json", schema_id=camera_schema_id), + 'camera_tpp': writer.register_channel(topic="/camera_tpp", message_encoding="json", schema_id=camera_schema_id), + 'lidar': writer.register_channel(topic="/lidar", message_encoding="json", schema_id=lidar_schema_id), + 'native_radar': writer.register_channel(topic="/radar/native", message_encoding="json", schema_id=lidar_schema_id), + 'ego_pose': writer.register_channel(topic="/ego_pose", message_encoding="json", schema_id=pose_schema_id) + } + + shenron_channels = {} + for r_type in radar_types: + shenron_channels[r_type] = writer.register_channel(topic=f"/radar/{r_type}", message_encoding="json", schema_id=lidar_schema_id) + + try: + frames_gen = load_frames(logs_dir) + frames = list(frames_gen) + except Exception as e: + print(f"[ERROR] Could not load frames.jsonl from {logs_dir}: {e}") + return + + for frame in tqdm.tqdm(frames, desc=" Packaging Frames", unit="frame"): + ts_ns = int(frame["timestamp"] * 1e9) + ts_sec = ts_ns // 1_000_000_000 + ts_nsec = ts_ns % 1_000_000_000 + + raw_pose = frame["ego_pose"] + x, y, z = raw_pose["x"], -raw_pose["y"], raw_pose["z"] + yaw_rad = -np.radians(raw_pose.get("yaw", 0)) + + ego_world_pose = {"position": {"x": x, "y": y, "z": z}, "orientation": {"x": 0.0, "y": 0.0, "z": float(np.sin(yaw_rad/2)), "w": float(np.cos(yaw_rad/2))}} + identity_pose = {"position": {"x": 0.0, "y": 0.0, "z": 0.0}, "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}} + + # POSE + writer.add_message(channels['ego_pose'], log_time=ts_ns, data=json.dumps(ego_world_pose).encode(), publish_time=ts_ns) + + # CAMERA + cam_path = logs_dir / "camera" / frame["camera"] + if cam_path.exists(): + with open(cam_path, "rb") as img_f: img_bytes = img_f.read() + cam_msg = {"timestamp": {"sec": ts_sec, "nsec": ts_nsec}, "frame_id": "ego_vehicle", "format": "png", "data": base64.b64encode(img_bytes).decode("ascii")} + writer.add_message(channels['camera'], log_time=ts_ns, data=json.dumps(cam_msg).encode(), publish_time=ts_ns) + + # CAMERA TPP + if "camera_tpp" in frame: + cam_tpp_path = logs_dir / "camera_tpp" / frame["camera_tpp"] + if cam_tpp_path.exists(): + with open(cam_tpp_path, "rb") as img_f: img_bytes = img_f.read() + cam_msg = {"timestamp": {"sec": ts_sec, "nsec": ts_nsec}, "frame_id": "ego_vehicle", "format": "png", "data": base64.b64encode(img_bytes).decode("ascii")} + writer.add_message(channels['camera_tpp'], log_time=ts_ns, data=json.dumps(cam_msg).encode(), publish_time=ts_ns) + + # LIDAR + lidar_p = logs_dir / "lidar" / frame["lidar"] + if lidar_p.exists(): + points = np.load(lidar_p) + # Robustness: Handle 6-column (Old) vs 7-column (Modern with Velocity) data + if points.shape[1] == 6: + # Pad to 7-column [x, y, z, vel, cos, obj, tag] + # Note: obj and tag columns [4,5] are actually uint32 bitstreams + padded = np.zeros((points.shape[0], 7), dtype=np.float32) + padded[:, 0:3] = points[:, 0:3] + padded[:, 4] = points[:, 3] # cos (pure float) + padded[:, 5] = points[:, 4].view(np.uint32).astype(np.float32) # real object id + padded[:, 6] = points[:, 5].view(np.uint32).astype(np.float32) # real semantic tag + ros_points = padded + else: + ros_points = points.copy().astype(np.float32) + # For newer 7-col data: [x,y,z,vel,cos,obj,tag] + # We still need to fix the obj/tag bits at [5,6] + ros_points[:, 5] = ros_points[:, 5].view(np.uint32).astype(np.float32) + ros_points[:, 6] = ros_points[:, 6].view(np.uint32).astype(np.float32) + + ros_points[:, 1] = -ros_points[:, 1] # RHS conversion + + # MOUNT OFFSET: LiDAR is on the roof (Z=2.5) + lidar_pose = {"position": {"x": 0.0, "y": 0.0, "z": 2.5}, "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}} + + lidar_msg = { + "timestamp": {"sec": ts_sec, "nsec": ts_nsec}, "frame_id": "ego_vehicle", "pose": lidar_pose, "point_stride": 28, + "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}, + {"name":"cos_inc_angle","offset":16,"type":7}, + {"name":"object_id","offset":20,"type":7}, + {"name":"semantic_tag","offset":24,"type":7} + ], + "data": base64.b64encode(ros_points.tobytes()).decode("ascii") + } + writer.add_message(channels['lidar'], log_time=ts_ns, data=json.dumps(lidar_msg).encode(), publish_time=ts_ns) + + # NATIVE RADAR + radar_p = logs_dir / "radar" / frame["radar"] + if radar_p.exists(): + r_data = np.load(radar_p) + if r_data.size > 0: + 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) + radar_points = np.stack([xr, yr, zr, vel], axis=1).astype(np.float32) + + # MOUNT OFFSET: Native Radar is on the front bumper (X=2.0, Z=1.0) + radar_pose = {"position": {"x": 2.0, "y": 0.0, "z": 1.0}, "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}} + + radar_msg = { + "timestamp": {"sec": ts_sec, "nsec": ts_nsec}, "frame_id": "ego_vehicle", "pose": radar_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(channels['native_radar'], log_time=ts_ns, data=json.dumps(radar_msg).encode(), publish_time=ts_ns) + + # SHENRON RADARS + shenron_fname = f"frame_{int(frame['frame_id']):06d}.npy" + for r_type in radar_types: + s_path = iter_dir / r_type / shenron_fname + if s_path.exists(): + s_data = np.load(s_path) + if s_data.size > 0: + ros_shenron = s_data.copy().astype(np.float32) + ros_shenron[:, 1] = -ros_shenron[:, 1] # Negate Y for ROS + # MOUNT OFFSET: Shenron Radars use the same pose as native (X=2.0, Z=1.0) + shenron_pose = {"position": {"x": 2.0, "y": 0.0, "z": 1.0}, "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}} + + shenron_msg = { + "timestamp": {"sec": ts_sec, "nsec": ts_nsec}, "frame_id": "ego_vehicle", "pose": shenron_pose, "point_stride": 20, + "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}, {"name":"magnitude","offset":16,"type":7}], + "data": base64.b64encode(ros_shenron.tobytes()).decode("ascii") + } + writer.add_message(shenron_channels[r_type], log_time=ts_ns, data=json.dumps(shenron_msg).encode(), publish_time=ts_ns) + + writer.finish() + print(f"\n[SUCCESS] Iteration packaged to: {output_mcap}") + print(f"File size: {os.path.getsize(output_mcap)/1024/1024:.2f} MB") + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Shenron Physics Iteration Testbench") + parser.add_argument("--iter", required=True, help="Name of the current debug iteration (e.g., 01_baseline)") + args = parser.parse_args() + + run_testbench(args.iter) diff --git a/src/recorder.py b/src/recorder.py index e2df95f..8cc69be 100644 --- a/src/recorder.py +++ b/src/recorder.py @@ -85,8 +85,53 @@ class Recorder: total_points = len(lidar) if total_points > 0: lidar_data = np.reshape(lidar_data, (total_points, -1)) + + # --- CALCULATE RADIAL VELOCITY FOR SHENRON --- + world = vehicle.get_world() + + # 1. Ego Velocity in local Sensor coordinates (approx matched with Ego) + ego_vel = vehicle.get_velocity() + yaw_rad = math.radians(vehicle.get_transform().rotation.yaw) + c, s = math.cos(yaw_rad), math.sin(yaw_rad) + ego_vx_local = ego_vel.x * c + ego_vel.y * s + ego_vy_local = -ego_vel.x * s + ego_vel.y * c + ego_vel_local = np.array([ego_vx_local, ego_vy_local, ego_vel.z], dtype=np.float32) + + # 2. V_rel for each point (static by default) + V_rel_all = np.zeros((total_points, 3), dtype=np.float32) + V_rel_all[:] = -ego_vel_local + + # A correctly interpreted bitview to extract integer IDs + obj_ids = lidar_data[:, 4].astype(np.float32).view(np.uint32) + unique_hit_ids = np.unique(obj_ids) + + for act_id in unique_hit_ids: + if act_id == 0: continue + act = world.get_actor(int(act_id)) + if act is not None: + act_vel = act.get_velocity() + ax_l = act_vel.x * c + act_vel.y * s + ay_l = -act_vel.x * s + act_vel.y * c + az_l = act_vel.z + V_rel_all[obj_ids == act_id] = np.array([ax_l, ay_l, az_l], dtype=np.float32) - ego_vel_local + + # 3. Project to Line of Sight (LOS) unit vector + pts = lidar_data[:, 0:3] + ranges = np.linalg.norm(pts, axis=1) + safe_ranges = np.where(ranges < 0.001, 1.0, ranges) + unit_LOS = pts / safe_ranges[:, None] + + # A positive radial_speed means distance is increasing (away from sensor) + radial_speed = np.sum(V_rel_all * unit_LOS, axis=1, keepdims=True) + + # 4. Inject speed cleanly: [x, y, z, radial_speed, cos, obj, tag] + lidar_data = np.hstack(( + lidar_data[:, 0:3], + radial_speed, + lidar_data[:, 3:6] + )) else: - lidar_data = np.empty((0, 6)) # Default to 6 columns for semantic + lidar_data = np.empty((0, 7)) # Default to 7 columns for semantic + velocity lidar_file = f"frame_{self.frame_id:06d}.npy" lidar_path = os.path.join(self.lidar_path, lidar_file) diff --git a/tmp/check_lidar.py b/tmp/check_lidar.py new file mode 100644 index 0000000..0c73f23 --- /dev/null +++ b/tmp/check_lidar.py @@ -0,0 +1,14 @@ +import numpy as np +import os +import glob +from pathlib import Path + +path = Path(r"d:\CARLA\CARLA_0.9.16\PythonAPI\Fox\Shenron_debug\logs\lidar") +files = sorted(list(path.glob('*.npy')))[:5] + +for f in files: + data = np.load(f) + print(f"\n[FILE: {f.name} | SHAPE: {data.shape}]") + for i in range(data.shape[1]): + col = data[:, i] + print(f" Col {i}: min={col.min():4.3f} | max={col.max():4.3f} | unique={len(np.unique(col)):5d}")