feat: Integrate 1843 radar profile with BPM support and ADC export

scripts/ISOLATE/ConfigureRadar/ConfigureRadar.py

@@ -0,0 +1,162 @@
+import numpy as np
+# from numba.experimental import jitclass
+from mat4py import loadmat
+import pdb
+
+class radar():
+
+    """
+    class to define the radar object with it's settings and to extract time intervals for updating the scene.
+    Also contains voxel filter specs
+    """
+    def __init__(self, radartype):
+        self.radartype = radartype
+        # Defaults for single-TX operation; overridden by specific radar profiles.
+        self.nTx = 1
+        self.total_tx = 1
+        self.elevation_tx_index = None
+        self.active_tx_indices = [0]
+        self.bpm_mode = 0
+        self.bpm_decode = 0
+        self.tx_positions_m = np.array([0.0])
+
+        if radartype == "ti_cascade":
+            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 = 0.256e9  # Bandwidth
+            self.c = 3e8
+            self.N_sample = 256
+            self.samp_rate = 15e6
+            self.doppler_mode = 1
+            self.chirps = 3  # 128
+            self.nRx = 86 #16  # number of antennas(virtual antennas included, AOA dim)
+            self.noise_amp = 0.005  #0.0001(concrete+metal)  # 0.00001(metal) #0.005(after skyward data)
+            self.gain = 10 ** (105 / 10)  #190(concrete+metal)  # 210(metal)
+            self.angle_fft_size = 256
+
+            self.range_res = self.c / (2 * self.B)  # range resolution
+            self.max_range = self.range_res * self.N_sample
+
+            self.idle = 0 ## Idle time
+            self.chirpT = self.N_sample / self.samp_rate  ## Time of chirp
+            self.chirp_rep = 12*27e-6
+
+            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
+
+        elif radartype == "ti_mrr_bpm_2tx4rx":
+            # TI MRR-like profile with 2TX BPM MIMO over 4 RX.
+            # Uses azimuth TXs (TX1, TX3). Elevated TX (TX2) is excluded.
+            self.center = np.array([0.0, 4.0])
+            self.elv = np.array([0.5])
+            self.orientation = 90
+
+            self.f = 76.01e9
+            self.B = 4.0e6 * 60e-6  # slope(4MHz/us) * ramp(60us) ~= 240MHz
+            self.c = 3e8
+            self.N_sample = 256
+            self.samp_rate = 4.652e6
+            self.doppler_mode = 1
+            # 256 physical chirps -> 128 BPM-decoded Doppler bins.
+            self.chirps = 256
+            self.nRx = 4
+            self.nTx = 2
+            self.total_tx = 3
+            self.elevation_tx_index = 1
+            self.active_tx_indices = [0, 2]  # TX1 and TX3
+            self.bpm_mode = 1
+            self.bpm_decode = 0
+            self.noise_amp = 0.005
+            self.gain = 10 ** (105 / 10)
+            self.angle_fft_size = 32
+
+            self.range_res = self.c / (2 * self.B)
+            self.max_range = self.range_res * self.N_sample
+
+            # TI values are in 10 ns LSB: idle=500, ramp_end=6000 -> 5us and 60us.
+            self.idle = 500 * 10e-9
+            self.chirpT = 6000 * 10e-9
+            # PRI for chirp type 0 with BPM (multiply by number of TXs).
+            self.chirp_rep = self.nTx * (self.idle + self.chirpT)
+
+            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
+
+            _lambda = self.c / self.f
+            # TX1/TX3 spacing in azimuth is approximated as lambda in this simulator.
+            self.tx_positions_m = np.array([0.0, _lambda])
+
+            self.voxel_theta = 2.0
+            self.voxel_phi = 2.0
+            self.voxel_rho = 0.05
+
+        elif radartype == "radarbook":
+            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 = 24e9
+            self.B = 0.250e9  # Bandwidth
+            self.c = 3e8
+            self.N_sample = 256
+            self.samp_rate = 1e6
+            self.doppler_mode = 1
+            self.chirps = 128
+            self.nRx = 8  # number of antennas(virtual antennas included, AOA dim)
+            self.noise_amp = 0.005  #0.0001(concrete+metal)  # 0.00001(metal) #0.005(after skyward data)
+            self.gain = 10 ** (105 / 10)  #190(concrete+metal)  # 210(metal)
+            self.angle_fft_size = 256
+
+            self.range_res = self.c / (2 * self.B)  # range resolution
+            self.max_range = self.range_res * self.N_sample
+
+            
+            self.chirpT = self.N_sample / self.samp_rate  ## Time of chirp
+            self.chirp_rep = 0.75e-3
+            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.5  # 0.1
+            self.voxel_phi = 2  # 0.5  # 0.1
+            self.voxel_rho = 0.05  # 0.1  # 0.05
+
+        else:
+            raise Exception("Incorrect radartype selected")
+
+    def get_noise(self):
+        if self.radartype == "ti_cascade": 
+            # noise_prop = loadmat('/radar-imaging-dataset/mmfn_project/mmfn_scripts/team_code/e2e_agent_sem_lidar2shenron_package/noise_data/noise_adc.mat')
+            # real_fft_ns = np.random.normal(noise_prop['noise_mean_real'], noise_prop['noise_std_real']).T
+            # complex_fft_ns = np.random.normal(noise_prop['noise_mean_real'], noise_prop['noise_std_real']).T
+            # final_noise = real_fft_ns + 1j * complex_fft_ns
+            # # signal_Noisy = np.fft.ifft(final_noise, radar.N_sample, 1) #* 10**4.5
+            # # signal_Noisy = final_noise 
+            # signal_Noisy = 0*final_noise 
+            
+            # 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)
+
+        elif self.radartype == "radarbook":
+            signal_Noisy = np.random.normal(0,1,size=(self.nRx,self.N_sample))
+            signal_Noisy = 0*(signal_Noisy + 1j*signal_Noisy)
+
+        else:
+            raise Exception("Incorrect radartype selected")
+
+        return signal_Noisy

scripts/ISOLATE/ConfigureRadar/heatmap_gen_fast.py

@@ -0,0 +1,299 @@
+import numpy as np
+from e2e_agent_sem_lidar2shenron_package.ConfigureRadar import radar
+import matplotlib.pyplot as plt
+from matplotlib import cm
+from mat4py import loadmat
+import math
+from mpl_toolkits.mplot3d import axes3d, Axes3D
+import time
+import scipy.io as sio
+# from numba import jit
+# from numba import int32, float64, complex128
+import pdb
+import torch
+from pynvml import *
+
+def get_gpu_id_most_avlbl_mem():
+
+    nvmlInit()
+    deviceCount = nvmlDeviceGetCount()
+    free = []
+    for i in range(deviceCount):
+        h = nvmlDeviceGetHandleByIndex(i)
+        info = nvmlDeviceGetMemoryInfo(h)
+        free.append(info.free)
+    free = np.array(free)
+    # h0 = nvmlDeviceGetHandleByIndex(0)
+    # h1 = nvmlDeviceGetHandleByIndex(1)
+    # h2 = nvmlDeviceGetHandleByIndex(2)
+    # h3 = nvmlDeviceGetHandleByIndex(3)
+    # info0 = nvmlDeviceGetMemoryInfo(h0)
+    # info1 = nvmlDeviceGetMemoryInfo(h1)
+    # info2 = nvmlDeviceGetMemoryInfo(h2)
+    # info3 = nvmlDeviceGetMemoryInfo(h3)
+    
+    # free = np.array([info0.free,info1.free,info2.free,info3.free])
+    
+    return np.argmax(free), np.max(free)
+
+# @jit(nopython=True)
+def heatmap_gen(rho, theta, loss, speed, radar, plot_fig, return_power):
+    start = time.time()
+    range_res = radar.c / (2 * radar.B)
+    max_range = range_res * radar.N_sample
+
+    Ts = 1 / radar.samp_rate
+    t = np.arange(0, radar.chirpT, Ts)
+    tau_resolution = 1 / radar.B
+    k = radar.B / radar.chirpT
+    x = np.exp(1j * 2 * np.pi * (radar.f + 0.5 * k * t) * t)
+
+
+    _lambda = radar.c / radar.f
+    sRx = _lambda / 2
+    _lambda = radar.c / radar.f
+
+    # Declare if we should use cpu or cuda
+    gpu_id, _ = get_gpu_id_most_avlbl_mem()
+    # pdb.set_trace()
+    device = torch.device(f'cuda:{gpu_id}' if torch.cuda.is_available() else 'cpu')
+    # device = torch.device('cpu')
+    print(f"------- Using {device}:{gpu_id} -------") 
+
+    # beamforming_vector_constant = np.zeros((radar.nRx,rho.shape[0]))
+    delta = torch.zeros((radar.nRx,rho.shape[0]),device= device)
+
+    bpm_mode = bool(getattr(radar, 'bpm_mode', 0))
+    n_tx = int(getattr(radar, 'nTx', 1))
+    tx_positions = np.asarray(getattr(radar, 'tx_positions_m', np.array([0.0])), dtype=float)
+    if tx_positions.shape[0] != n_tx:
+        tx_positions = np.zeros((n_tx,), dtype=float)
+    
+
+    ##Noise
+    # noise_prop = loadmat('noise.mat')
+    # real_fft_ns = np.random.normal(noise_prop['noise_mean_real'], noise_prop['noise_std_real']).T
+    # complex_fft_ns = np.random.normal(noise_prop['noise_mean_real'], noise_prop['noise_std_real']).T
+    # final_noise = real_fft_ns + 1j * complex_fft_ns
+    # signal_Noisy = np.fft.ifft(final_noise, radar.N_sample, 1) * 10
+    signal_Noisy = radar.get_noise()
+
+    ##initialising on torch
+    loss = torch.tensor(loss*(1 / rho ** 2), device=device).float()
+    rho = torch.tensor(rho, device=device).float()
+    signal_Noisy = torch.tensor(signal_Noisy, device=device)
+    theta = torch.tensor(theta, device=device).float()
+    speed = torch.tensor(speed, device=device).float()
+    t = torch.tensor(t, device=device).float()
+    
+    torch.set_printoptions(precision=10)
+    if radar.doppler_mode:
+        measurement = np.zeros((radar.chirps, radar.nRx, radar.N_sample), dtype="complex128")  # doppler,AoA,range
+        # start = time.time()
+        for chirp in range(radar.chirps):
+            tau = 2 * (rho / radar.c + chirp * (radar.chirp_rep) * speed / radar.c)
+            # tau = 2 * rho / radar.c + chirp * (12*27e-6) * speed / radar.c
+            # print(tau[torch.argmax(abs(speed))])            
+            sin_term = torch.sin(np.pi / 2 - theta)
+            for i in range(radar.nRx):
+                delta[i,:] = i * sRx * sin_term / radar.c
+
+            # delta = torch.tensor(delta,device='cuda')
+            # tau = torch.tensor(tau,device='cuda')
+            # t = torch.tensor(t,device='cuda')
+
+            # beamforming_vector = np.exp(1j*2*np.pi*(radar.f*delta[:,:,None] - 0.5*k*delta[:,:,None]**2 - k*tau[None,:,None]*delta[:,:,None] + k*delta[:,:,None]@t[None,None,:])) #(80xN)
+            dechirped = torch.exp((1j * 2 * np.pi) *(radar.f * tau[:,None] + k * tau[:,None]@t[None,:] - 0.5 * k * tau[:,None]**2))
+            # loss_factor = (loss * (1 / rho ** 2))[:,None] ## from network
+            loss_factor = (torch.sqrt(loss)[:,None]) ## from network
+
+            signal_single_antenna = loss_factor*dechirped # (Nx256)
+
+            # BPM chirp coding for 2TX mode:
+            # even chirp: [+1, +1], odd chirp: [+1, -1]
+            if bpm_mode and n_tx == 2:
+                if chirp % 2 == 0:
+                    bpm_codes = [1.0, 1.0]
+                else:
+                    bpm_codes = [1.0, -1.0]
+            else:
+                bpm_codes = [1.0] * n_tx
+
+            signal = torch.zeros((radar.nRx, radar.N_sample), device=device, dtype=torch.complex64)
+            for tx_idx in range(n_tx):
+                tx_delay = torch.tensor(tx_positions[tx_idx], device=device, dtype=torch.float32) * sin_term / radar.c
+                tx_delta = delta + tx_delay[None, :]
+                beamforming_vector = torch.exp(
+                    1j * 2 * np.pi * (
+                        radar.f * tx_delta[:, :, None]
+                        - 0.5 * k * tx_delta[:, :, None] ** 2
+                        - k * tau[None, :, None] * tx_delta[:, :, None]
+                        + k * tx_delta[:, :, None] @ t[None, None, :]
+                    )
+                )
+                tx_signal = beamforming_vector * signal_single_antenna[None, :, :]
+                tx_signal = torch.squeeze(torch.sum(tx_signal, 1))
+                signal = signal + bpm_codes[tx_idx] * tx_signal
+
+            # pdb.set_trace()
+
+                    # (np.expand_dims(t,0) - np.expand_dims(tau_vec[:,j],1))) * (
+                    #     np.expand_dims(t,0) - np.expand_dims(tau_vec[:,j],1)))
+
+            # signal = np.exp(1j*2*np.pi * ())
+
+            # noise_real = (1j*1j*-1) * (2 * np.random.rand(radar.nRx, radar.N_sample) - 1)
+            # noise_complex = (1j) * (2 * np.random.rand(radar.nRx, radar.N_sample) - 1)
+            # noise = (noise_real+noise_complex) * radar.noise_amp
+            # signal_Noisy = signal
+
+            # sum_samp = sum_samp + signal_Noisy ## Here I am not adding individual noise for each point
+            
+
+
+            # adc_sampled = np.sqrt(radar.gain * _lambda ** 2 / (4 * np.pi) ** 3) * np.conj(signal_Noisy) * (x)
+            adc_sampled = torch.sqrt(torch.tensor(radar.gain * _lambda ** 2 / (4 * np.pi) ** 3)) * signal
+            adc_sampled = adc_sampled + signal_Noisy
+            
+            adc_sampled = adc_sampled.cpu().numpy()
+
+            
+
+            measurement[chirp, :, :] = adc_sampled
+        # pdb.set_trace()
+        del rho
+        del loss
+        # del signal_noisy
+        del speed
+        del theta
+        del t
+        del dechirped
+        del signal
+        del signal_single_antenna
+        with torch.cuda.device(f'cuda:{gpu_id}'):
+            torch.cuda.empty_cache()
+        # Optionally decode BPM into virtual channels (2TX x nRx).
+        if bpm_mode and n_tx == 2 and bool(getattr(radar, 'bpm_decode', 0)):
+            n_pairs = radar.chirps // 2
+            even_chirps = measurement[0:2*n_pairs:2, :, :]
+            odd_chirps = measurement[1:2*n_pairs:2, :, :]
+            tx0 = 0.5 * (even_chirps + odd_chirps)
+            tx1 = 0.5 * (even_chirps - odd_chirps)
+            measurement = np.concatenate((tx0, tx1), axis=1)
+
+        # pdb.set_trace()
+        return measurement
+        # end = time.time()
+        # diction = {"doppler_cube": measurement}
+        # sio.savemat("E:/Radar_sim/simlator/git repo/Heatmap-sim/doppler_cube.mat", diction)
+#         RangeFFT = np.fft.fft(measurement, radar.N_sample, 2)
+#         AngleFFT = np.fft.fftshift(np.fft.fft(RangeFFT[0, :, :], radar.angle_fft_size, 0), 0)
+#         Doppler_data = np.fft.fftshift(np.fft.fft(np.fft.fftshift(np.fft.fft(RangeFFT, radar.angle_fft_size, 1), 1),
+#                                                   radar.chirps, 0), 0)
+#         Doppler_heatmap = np.sum(np.arange(radar.chirps)[:, None, None] * np.abs(Doppler_data), axis=0) / np.sum(
+#             np.abs(Doppler_data), axis=0) - radar.chirps / 2
+            
+    else:
+        tau = 2 * rho / radar.c
+        _lambda = radar.c / radar.f
+        sRx = _lambda / 2  # separation
+        _lambda = radar.c / radar.f
+
+        tau_vec = np.zeros((radar.nRx, rho.shape[0]))
+        for i in range(radar.nRx):
+            tau_vec[i, :] = tau + (i) * sRx * np.sin(np.pi / 2 - theta) / radar.c
+
+        sum_samp = np.zeros((radar.nRx, radar.N_sample), dtype="complex128")
+        for j in range(rho.shape[0]):
+            if (rho[j] != 0):
+                if return_power:
+                    if rho[j] != 0:
+                        signal = 10**10 * np.sqrt(loss[j]) * np.exp(
+                            1j * 2 * np.pi * (radar.f + 0.5 * k * (np.expand_dims(t,0) - np.expand_dims(tau_vec[:,j],1))) * (
+                                    np.expand_dims(t,0) - np.expand_dims(tau_vec[:,j],1)))
+                else:
+                    if rho[j] != 0:
+                        signal = loss[j] * (1 / rho[j] ** 2) * np.exp(
+                            1j * 2 * np.pi * (radar.f + 0.5 * k * (np.expand_dims(t,0) - np.expand_dims(tau_vec[:,j],1))) * (
+                                    np.expand_dims(t,0) - np.expand_dims(tau_vec[:,j],1)))
+
+                noise_real = (1j*1j*-1) * (2 * np.random.rand(radar.nRx, radar.N_sample) - 1)
+                noise_complex = (1j) * (2 * np.random.rand(radar.nRx, radar.N_sample) - 1)
+                noise = (noise_real+noise_complex) * radar.noise_amp
+                signal_Noisy = signal + noise
+
+                sum_samp = sum_samp + signal_Noisy*10**5
+
+        adc_sampled = np.sqrt(radar.gain * _lambda ** 2 / (4 * np.pi) ** 3) * np.conj(sum_samp) * (x)
+        # RangeFFT = np.fft.fft(adc_sampled, radar.N_sample, 1)
+
+        # pwr_prof = 10*np.log10(np.sum(abs(RangeFFT)**2, 0)+1)
+        # plt.plot(radar.range_res*np.arange(radar.N_sample), pwr_prof)
+        # plt.axis([0, 256*radar.range_res, 70, 180])
+        # plt.show()
+
+        # AngleFFT = np.fft.fftshift(np.fft.fft(RangeFFT, radar.angle_fft_size, 0), 0)
+
+        # Doppler_heatmap = np.zeros(np.shape(AngleFFT))
+
+        return measurement
+
+
+    # print('runtime is ', end - start)
+
+#     d = 1
+#     sine_theta = -2 * np.arange(-radar.angle_fft_size / 2, (radar.angle_fft_size / 2) + 1) / radar.angle_fft_size / d
+#     # sine_theta = -2*np.arange(-radar.angle_fft_size/2,(radar.angle_fft_size/2)+1)/radar.angle_fft_size/d
+#     cos_theta = np.sqrt(1 - sine_theta ** 2)
+#     indices_1D = np.arange(0, radar.N_sample)
+#     [R_mat, sine_theta_mat] = np.meshgrid(indices_1D * range_res, sine_theta)
+#     [_, cos_theta_mat] = np.meshgrid(indices_1D, cos_theta)
+
+#     x_axis = R_mat * cos_theta_mat
+#     y_axis = R_mat * sine_theta_mat
+#     mag_data_static = abs(np.vstack(
+#         (AngleFFT, AngleFFT[255, :])))  # np.column_stack((abs(AngleFFT[indices_1D,:]),abs(AngleFFT[indices_1D,0])))
+#     mag_data_doppler = abs(np.vstack((Doppler_heatmap, Doppler_heatmap[255, :])))
+#     # doppler_cube = np.concatenate((Doppler_data, Doppler_data[:, 255, :][:, np.newaxis, :]), 1)
+
+#     mag_data_static = np.flipud(mag_data_static)
+#     mag_data_doppler = np.flipud(mag_data_doppler)
+#     # doppler_cube = np.flipud(doppler_cube)
+
+#     return x_axis, y_axis, mag_data_static, mag_data_doppler
+       
+
+if __name__ == '__main__':
+    radar = radar()
+    radar.chirps = 1
+    radar.center = np.array([0.0, 0.0])  # center of radar
+    radar.elv = np.array([0.0])
+
+    test_WI=1
+    if (test_WI):
+        # points = np.load("E:/Radar_sim/simlator/git repo/Heatmap-sim/simulation5.npy")
+        cell_array = sio.loadmat("wi_data/single_radar_wi_10m_allangles.mat")
+        
+        for i in range(36):
+            print(f"Simulation: {i}")
+            points = cell_array['cell_array'][i][0]
+            if points.shape[0]==0:
+                print("Skipped")
+                continue
+            rho = np.linalg.norm(points[:, 0:3], axis=1)
+            theta = math.pi / 2 - np.arctan(((points[:, 0] - radar.center[0]) / (points[:, 1] - radar.center[1])))
+            loss = 10**(points[:, 3]/20)
+            speed = np.zeros_like(rho)
+    
+
+            adc_data = heatmap_gen(rho, theta, loss, speed, radar, 1, 0)
+            diction = {"adc_data": adc_data}
+            sio.savemat(f"wi_data/simulation_{i}.mat", diction)
+
+    # fig, ax = plt.subplots(subplot_kw={"projection": "3d"})
+    # ax.view_init(elev=90, azim=180)
+    # surf = ax.plot_surface(x_axis, y_axis, plot_data ** 0.7, cmap=cm.coolwarm,
+    #                        linewidth=0, antialiased=False)
+    # plt.show()
+
+

scripts/ISOLATE/ConfigureRadar/lidar.py

@@ -0,0 +1,242 @@
+import sys
+import os
+# sys.path.append("../")
+sys.path.append('radar-imaging-dataset/carla_garage_radar/team_code/e2e_agent_sem_lidar2shenron_package/')
+
+import numpy as np
+from e2e_agent_sem_lidar2shenron_package.path_config import *
+from e2e_agent_sem_lidar2shenron_package.ConfigureRadar import radar
+from e2e_agent_sem_lidar2shenron_package.shenron.Sceneset import *
+from e2e_agent_sem_lidar2shenron_package.shenron.heatmap_gen_fast import *
+# from pointcloud_raytracing.pointraytrace import ray_trace
+import scipy.io as sio
+from e2e_agent_sem_lidar2shenron_package.lidar_utils import *
+import time
+import shutil
+import pdb
+
+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
+    '''
+    # print(carla_sem_lidar_data.shape())
+    carla_sem_lidar_data_crop = carla_sem_lidar_data[:, (0, 1, 2, 5)]
+    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
+
+# def map_carla_semantic_lidar(carla_sem_lidar_data):
+#     '''
+#     Function to map material column in the collected carla ray_cast_shenron to shenron input 
+#     '''
+#     # print(carla_sem_lidar_data.shape())
+#     carla_sem_lidar_data_crop = carla_sem_lidar_data[:, (0, 1, 2, 5)]
+#     carla_sem_lidar_data_crop[:, 3] = carla_sem_lidar_data_crop[:, 3]-1
+#     carla_sem_lidar_data_crop[carla_sem_lidar_data_crop[:, 3]>18, 3] = 255.
+#     carla_sem_lidar_data_crop[carla_sem_lidar_data_crop[:, 3]<0, 3] = 255.
+#     carla_sem_lidar_data_crop[:, (0, 1, 2)] = carla_sem_lidar_data_crop[:, (0, 2, 1)]
+#     return carla_sem_lidar_data_crop
+
+def check_save_path(save_path):
+    if not os.path.exists(save_path):
+        os.makedirs(save_path)
+    return
+
+def rotate_points(points, angle):
+    rotMatrix = np.array([[np.cos(np.deg2rad(angle)), np.sin(np.deg2rad(angle)), 0]
+        , [- np.sin(np.deg2rad(angle)), np.cos(np.deg2rad(angle)), 0]
+        , [0, 0, 1]])
+    return np.matmul(points, rotMatrix)
+
+def Cropped_forRadar(pc, veh_coord, veh_angle, radarobj):
+    """
+    Removes Occlusions and calculates loss for each point
+    """
+
+    skew_pc = rotate_points(pc[:, 0:3] , veh_angle )
+    # 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))
+    new_pc = skew_pc[rowy, :].squeeze(0)
+
+    new_pc = new_pc[new_pc[:,4]!=0]
+
+    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)]
+
+    simobj = Sceneset(new_pc)
+
+    [rho, theta, loss, speed, angles] = simobj.specularpoints(radarobj)
+    print(f"Number of points = {rho.shape[0]}")
+    return rho, theta, loss, speed, angles
+
+def run_lidar(sim_config, sem_lidar_frame):
+
+    #restructed lidar.py code
+
+    # lidar_path = f'{base_folder}/{sim_config["CARLA_SHENRON_SEM_LIDAR"]}'
+    # # lidar_velocity_path = f'{base_folder}/{sim_config["LIDAR_PATH_POINT_VELOCITY"]}/'
+    # out_path = f'{base_folder}/{sim_config["RADAR_PATH_SIMULATED"]}'
+
+    # if not os.path.exists(out_path):
+    #     os.makedirs(out_path)
+
+    # shutil.copyfile('ConfigureRadar.py',f'{base_folder}/radar_params.py')
+
+    # lidar_files = os.listdir(lidar_path)
+    # lidar_velocity_files = os.listdir(lidar_velocity_path)
+    # lidar_files.sort()
+    # lidar_velocity_files.sort()
+
+    # print(lidar_files)
+    
+    #Lidar specific settings
+    radarobj = radar(sim_config["RADAR_TYPE"])
+    if "BPM_DECODE" in sim_config:
+        radarobj.bpm_decode = int(sim_config["BPM_DECODE"])
+    # radarobj.chirps = 128
+    radarobj.center = np.array([0.0, 0.0])  # center of radar
+    radarobj.elv = np.array([0.0])
+
+    # setting the sem lidar inversion angle here
+    veh_angle = sim_config['INVERT_ANGLE']
+    
+    # all_speeds = []
+
+    # temp_angles = []
+    # temp_rho = []
+    # for file_no, file in enumerate(lidar_files):
+        
+    #     start = time.time()
+    #     if file.endswith('.npy'):  # .pcd
+    #         print(file)
+            
+    #         lidar_file_path = os.path.join(f"{lidar_path}/", file)
+    #         load_pc = np.load(lidar_file_path)
+
+            # load_velocity = np.load(f'{lidar_velocity_path}/{file}')
+
+            # test = map_material(test)
+    cosines = sem_lidar_frame[:, 3]
+    load_pc = sem_lidar_frame
+    load_pc = map_carla_semantic_lidar_latest(load_pc)
+    test = new_map_material(load_pc)
+    
+    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 mapping
+    +ve ind 0 == right
+    +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
+
+    points[:, 5] = test[:, 3]
+    points[:, 6] = cosines
+    ### if jason carla lidar
+    # points = np.zeros((np.shape(test)[0], 6))
+    # points[:, [0, 1, 2]] = load_pc[:, [0, 1, 2]]
+    # points[:, 5] = 4
+    ##########
+
+    # if USE_DIGITAL_TWIN:
+    #     gt_label = gt[file_no,:]
+    #     points, veh_speeds = create_digital_twin(points, gt_label) ## This also claculates and outputs speed
+
+    #     all_speeds.append(veh_speeds)
+
+    # if sim_config["RADAR_MOVING"]:
+    #     # when the radar is moving, we add a negative doppler from all the points
+    #     if INDOOR:
+    #         curr_radar_speed = radar_speeds[file_no,:]
+
+    #         cos_theta = (points[:,1]/np.linalg.norm(points[:,:2],axis=1))
+    #         radial_speed = -np.linalg.norm(curr_radar_speed)*cos_theta
+
+    #         points[:,3] += radial_speed
+    #         points[:,5] = 4 ## harcoded 
+
+    
+    Crop_rho, Crop_theta, Crop_loss, Crop_speed, Crop_angles = Cropped_forRadar(points, np.array([0, 0, 0]), veh_angle, radarobj)
+
+    """ DEBUG CODE
+    spec_angle_thresh = 2*np.pi/180#*(1/rho)
+
+    print(f"Number of points < 2deg = {np.sum(abs(Crop_angles)<spec_angle_thresh)}")
+    temp_angles.append(np.sum(abs(Crop_angles)<spec_angle_thresh))
+    temp_rho.append(Crop_rho.shape[0])
+    continue
+    """
+    
+
+    if sim_config["RAY_TRACING"]:
+        rt_rho, rt_theta = ray_trace(points)
+
+        rt_loss = np.mean(Crop_loss)*np.ones_like(rt_rho)
+        rt_speed = np.zeros_like(rt_rho)
+        Crop_rho = np.append(Crop_rho, rt_rho)
+        Crop_theta = np.append(Crop_theta, rt_theta)
+        Crop_loss = np.append(Crop_loss, rt_loss)
+        Crop_speed = np.append(Crop_speed, rt_speed)
+
+    adc_data = heatmap_gen(Crop_rho, Crop_theta, Crop_loss, Crop_speed, radarobj, 1, 0)
+
+    if "EXPORT_ADC_MAT_PATH" in sim_config and sim_config["EXPORT_ADC_MAT_PATH"]:
+        export_mat_path = sim_config["EXPORT_ADC_MAT_PATH"]
+        export_dir = os.path.dirname(export_mat_path)
+        if export_dir:
+            os.makedirs(export_dir, exist_ok=True)
+        export_dict = {
+            "adc_data": adc_data,
+            "adc_shape": np.array(adc_data.shape, dtype=np.int32),
+            "adc_layout": "chirp_rx_sample"
+        }
+        sio.savemat(export_mat_path, export_dict)
+
+    return adc_data
+    # check_save_path(out_path)
+    # np.save(f'{out_path}/{file[:-4]}', adc_data)
+    # diction = {"adc_data": adc_data}
+    # sio.savemat(f"{out_path}/{file[:-4]}.mat", diction)
+    # sio.savemat(f"test_pc.mat", diction)
+    # print(f'Time: {time.time()-start}')
+    # np.save("all_speeds_no_micro.npy",np.array(all_speeds))
+    """ DEBUG CODE
+    fig, ax = plt.subplots(1,2)
+    ax[0].plot(temp_angles)
+    ax[1].plot(temp_rho)
+    
+    plt.plot(temp_rho)
+    plt.show()
+    pdb.set_trace()
+    """ 
+
+if __name__ == '__main__':
+
+    points = np.zeros((100,6))
+
+    points[:,5] = 4
+    
+    points[:,0] = 1
+    points[:,1] = np.linspace(0,15,100)
+    
+    points[:,3] = -0.5*np.cos(np.arctan2(points[:,0],points[:,1]))
+    radarobj = radar('radarbook')
+    # radarobj.chirps = 128
+    radarobj.center = np.array([0.0, 0.0])  # center of radar
+    radarobj.elv = np.array([0.0])
+
+    Crop_rho, Crop_theta, Crop_loss, Crop_speed = Cropped_forRadar(points, np.array([0, 0, 0]), 0, radarobj)
+    Crop_loss = np.ones_like(Crop_loss)
+    adc_data = heatmap_gen(Crop_rho, Crop_theta, Crop_loss, Crop_speed, radarobj, 1, 0)
+    diction = {"adc_data": adc_data}
+    sio.savemat(f"test_pc.mat", diction)

scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/ConfigureRadar.py

@@ -11,6 +11,16 @@ class radar():
     """
     def __init__(self, radartype):
         self.radartype = radartype
+        
+        # Defaults for single-TX operation; overridden by specific radar profiles.
+        self.nTx = 1
+        self.total_tx = 1
+        self.elevation_tx_index = None
+        self.active_tx_indices = [0]
+        self.bpm_mode = 0
+        self.bpm_decode = 0
+        self.tx_positions_m = np.array([0.0])
+
         if radartype == "ti_cascade":
             self.center = np.array([0.0, 4.0])  # center of radar
             self.elv = np.array([0.5])  # self position in z-axis
@@ -120,6 +130,55 @@ class radar():
             self.voxel_phi = 2.0  # 0.5  # 0.1
             self.voxel_rho = 0.05  # 0.1  # 0.05
 
+        elif radartype == "1843":
+            self.center = np.array([0.0, 4.0])
+            self.elv = np.array([0.5])
+            self.orientation = 90
+
+            self.f = 76.01e9
+            self.B = 4.0e6 * 60e-6  # slope(4MHz/us) * ramp(60us) ~= 240MHz
+            self.c = 3e8
+            self.N_sample = 256
+            self.samp_rate = 4.652e6
+            self.doppler_mode = 1
+            # 256 physical chirps -> 128 BPM-decoded Doppler bins.
+            self.chirps = 256
+            self.nRx = 4
+            self.nTx = 2
+            self.total_tx = 3
+            self.elevation_tx_index = 1
+            self.active_tx_indices = [0, 2]  # TX1 and TX3
+            self.bpm_mode = 1
+            self.bpm_decode = 0
+            self.noise_amp = 0.005
+            self.gain = 10 ** (105 / 10)
+            self.angle_fft_size = 32
+
+            self.range_res = self.c / (2 * self.B)
+            self.max_range = self.range_res * self.N_sample
+
+            # TI values are in 10 ns LSB: idle=500, ramp_end=6000 -> 5us and 60us.
+            self.idle = 500 * 10e-9
+            self.chirpT = 6000 * 10e-9
+            # PRI for chirp type 0 with BPM (multiply by number of TXs).
+            self.chirp_rep = self.nTx * (self.idle + self.chirpT)
+
+            Ts = 1 / self.samp_rate
+            self.t = np.arange(self.N_sample) * Ts
+            self.tau_resolution = 1 / self.B
+            self.k = self.B / self.chirpT
+
+            _lambda = self.c / self.f
+            # TX1/TX3 spacing in azimuth is approximated as lambda in this simulator.
+            self.tx_positions_m = np.array([0.0, _lambda])
+
+            self.vertical_beamwidth = 20.0
+            self.sensitivity_floor = 0.0
+
+            self.voxel_theta = 2.0
+            self.voxel_phi = 2.0
+            self.voxel_rho = 0.05
+
         else:
             raise Exception("Incorrect radartype selected")
 
@@ -129,7 +188,7 @@ class radar():
             signal_Noisy = np.random.normal(0,1,size=(self.nRx,self.N_sample))
             signal_Noisy = (signal_Noisy + 1j*np.random.normal(0,1,size=(self.nRx,self.N_sample))) * self.noise_amp
 
-        elif self.radartype == "radarbook" or self.radartype == "awrl1432":
+        elif self.radartype == "radarbook" or self.radartype == "awrl1432" or self.radartype == "1843":
             signal_Noisy = np.random.normal(0,1,size=(self.nRx,self.N_sample))
             signal_Noisy = (signal_Noisy + 1j*np.random.normal(0,1,size=(self.nRx,self.N_sample))) * self.noise_amp
 

scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/lidar.py

@@ -226,6 +226,19 @@ def run_lidar(sim_config, sem_lidar_frame, radarobj=None):
         Crop_speed = np.append(Crop_speed, rt_speed)
 
     adc_data = heatmap_gen(Crop_rho, Crop_theta, Crop_loss, Crop_speed, radarobj, 1, 0)
+
+    if "EXPORT_ADC_MAT_PATH" in sim_config and sim_config["EXPORT_ADC_MAT_PATH"]:
+        export_mat_path = sim_config["EXPORT_ADC_MAT_PATH"]
+        export_dir = os.path.dirname(export_mat_path)
+        if export_dir:
+            os.makedirs(export_dir, exist_ok=True)
+        export_dict = {
+            "adc_data": adc_data,
+            "adc_shape": np.array(adc_data.shape, dtype=np.int32),
+            "adc_layout": "chirp_rx_sample"
+        }
+        sio.savemat(export_mat_path, export_dict)
+
     return adc_data
     # check_save_path(out_path)
     # np.save(f'{out_path}/{file[:-4]}', adc_data)

scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/shenron/heatmap_gen_fast.py

@@ -45,7 +45,7 @@ def heatmap_gen(rho, theta, loss, speed, radar, plot_fig, return_power):
     max_range = range_res * radar.N_sample
 
     Ts = 1 / radar.samp_rate
-    t = np.arange(0, radar.chirpT, Ts)
+    t = np.arange(radar.N_sample) * Ts
     tau_resolution = 1 / radar.B
     k = radar.B / radar.chirpT
     x = np.exp(1j * 2 * np.pi * (radar.f + 0.5 * k * t) * t)
@@ -74,6 +74,11 @@ def heatmap_gen(rho, theta, loss, speed, radar, plot_fig, return_power):
     # beamforming_vector_constant = np.zeros((radar.nRx,rho.shape[0]))
     delta = torch.zeros((radar.nRx,rho.shape[0]),device= device)
     
+    bpm_mode = bool(getattr(radar, 'bpm_mode', 0))
+    n_tx = int(getattr(radar, 'nTx', 1))
+    tx_positions = np.asarray(getattr(radar, 'tx_positions_m', np.array([0.0])), dtype=float)
+    if tx_positions.shape[0] != n_tx:
+        tx_positions = np.zeros((n_tx,), dtype=float)
 
     ##Noise
     # noise_prop = loadmat('noise.mat')
@@ -95,25 +100,25 @@ def heatmap_gen(rho, theta, loss, speed, radar, plot_fig, return_power):
     if radar.doppler_mode:
         measurement = np.zeros((radar.chirps, radar.nRx, radar.N_sample), dtype="complex128")  # doppler,AoA,range
         # start = time.time()
+        # Precompute delta and tx_delays outside the chirp loop
+        sin_term = torch.sin(np.pi / 2 - theta)
+        for i in range(radar.nRx):
+            delta[i,:] = i * sRx * sin_term / radar.c
+
+        tx_delays = []
+        for tx_idx in range(n_tx):
+            tx_delays.append(torch.tensor(tx_positions[tx_idx], device=device, dtype=torch.float32) * sin_term / radar.c)
+
         for chirp in range(radar.chirps):
             tau = 2 * (rho / radar.c + chirp * (radar.chirp_rep) * speed / radar.c)
             # tau = 2 * rho / radar.c + chirp * (12*27e-6) * speed / radar.c
             # print(tau[torch.argmax(abs(speed))])            
-            for i in range(radar.nRx):
-                delta[i,:] = i * sRx * torch.sin(np.pi / 2 - theta) / radar.c
 
             # delta = torch.tensor(delta,device='cuda')
             # tau = torch.tensor(tau,device='cuda')
             # t = torch.tensor(t,device='cuda')
 
             # beamforming_vector = np.exp(1j*2*np.pi*(radar.f*delta[:,:,None] - 0.5*k*delta[:,:,None]**2 - k*tau[None,:,None]*delta[:,:,None] + k*delta[:,:,None]@t[None,None,:])) #(80xN)
-            beamforming_vector = torch.exp(1j*2*np.pi*(radar.f*delta[:,:,None] - 0.5*k*delta[:,:,None]**2 - k*tau[None,:,None]*delta[:,:,None] + k*delta[:,:,None]@t[None,None,:])) #(80xN)
-
-            
-            # tau = tau.cpu().numpy()
-            # t = t.cpu().numpy()
-            # beamforming_vector = beamforming_vector.cpu().numpy()
-            
 
             dechirped = torch.exp((1j * 2 * np.pi) *(radar.f * tau[:,None] + k * tau[:,None]@t[None,:] - 0.5 * k * tau[:,None]**2))
             # loss_factor = (loss * (1 / rho ** 2))[:,None] ## from network
@@ -121,9 +126,30 @@ def heatmap_gen(rho, theta, loss, speed, radar, plot_fig, return_power):
 
             signal_single_antenna = loss_factor*dechirped # (Nx256)
 
-            signal = beamforming_vector*signal_single_antenna[None,:,:] # (80x256)
-
-            signal = torch.squeeze(torch.sum(signal,1))
+            # BPM chirp coding for 2TX mode:
+            # even chirp: [+1, +1], odd chirp: [+1, -1]
+            if bpm_mode and n_tx == 2:
+                if chirp % 2 == 0:
+                    bpm_codes = [1.0, 1.0]
+                else:
+                    bpm_codes = [1.0, -1.0]
+            else:
+                bpm_codes = [1.0] * n_tx
+
+            signal = torch.zeros((radar.nRx, radar.N_sample), device=device, dtype=torch.complex64)
+            for tx_idx in range(n_tx):
+                tx_delta = delta + tx_delays[tx_idx][None, :]
+                beamforming_vector = torch.exp(
+                    1j * 2 * np.pi * (
+                        radar.f * tx_delta[:, :, None]
+                        - 0.5 * k * tx_delta[:, :, None] ** 2
+                        - k * tau[None, :, None] * tx_delta[:, :, None]
+                        + k * tx_delta[:, :, None] @ t[None, None, :]
+                    )
+                )
+                tx_signal = beamforming_vector * signal_single_antenna[None, :, :]
+                tx_signal = torch.sum(tx_signal, 1)
+                signal += bpm_codes[tx_idx] * tx_signal
 
             # pdb.set_trace()
 
@@ -142,7 +168,7 @@ def heatmap_gen(rho, theta, loss, speed, radar, plot_fig, return_power):
 
 
             # adc_sampled = np.sqrt(radar.gain * _lambda ** 2 / (4 * np.pi) ** 3) * np.conj(signal_Noisy) * (x)
-            adc_sampled = torch.sqrt(torch.tensor(radar.gain * _lambda ** 2 / (4 * np.pi) ** 3)) * signal
+            adc_sampled = torch.sqrt(torch.tensor(radar.gain * _lambda ** 2 / (4 * np.pi) ** 3, device=device)) * signal
             adc_sampled = adc_sampled + signal_Noisy
             
             adc_sampled = adc_sampled.cpu().numpy()
@@ -158,12 +184,21 @@ def heatmap_gen(rho, theta, loss, speed, radar, plot_fig, return_power):
         del theta
         del t
         del dechirped
-        del beamforming_vector
         del signal
         del signal_single_antenna
         if device.type == 'cuda':
             with torch.cuda.device(f'cuda:{gpu_id}'):
                 torch.cuda.empty_cache()
+        
+        # Optionally decode BPM into virtual channels (2TX x nRx).
+        if bpm_mode and n_tx == 2 and bool(getattr(radar, 'bpm_decode', 0)):
+            n_pairs = radar.chirps // 2
+            even_chirps = measurement[0:2*n_pairs:2, :, :]
+            odd_chirps = measurement[1:2*n_pairs:2, :, :]
+            tx0 = 0.5 * (even_chirps + odd_chirps)
+            tx1 = 0.5 * (even_chirps - odd_chirps)
+            measurement = np.concatenate((tx0, tx1), axis=1)
+
         # pdb.set_trace()
         return measurement
         # end = time.time()

scripts/ISOLATE/model_wrapper.py

@@ -68,6 +68,7 @@ class ShenronRadarModel:
         # Update FFT Cfg
         ur.fftCfg['NFFT'] = self.radar_obj.N_sample
         ur.fftCfg['NFFTVel'] = self.radar_obj.chirps
+        ur.fftCfg['NFFTAnt'] = getattr(self.radar_obj, 'angle_fft_size', 256)
         
         print(f"Synced global config: N={ur.radarCfg['N']}, Np={ur.radarCfg['Np']}, Ant={ur.radarCfg['NrChn']}")
         
@@ -96,6 +97,10 @@ class ShenronRadarModel:
             
             # 1. Physics-based Signal Generation (FMCW Chirps)
             # This generates the raw ADC samples [Np, N, Ant]
+            if "adc_raw_dir" in self.sim_config:
+                frame_id = self.sim_config.get("current_frame_id", "000000")
+                self.sim_config["EXPORT_ADC_MAT_PATH"] = os.path.join(self.sim_config["adc_raw_dir"], f"adc_{frame_id}.mat")
+
             adc_data = run_lidar(self.sim_config, semantic_lidar_data, radarobj=self.radar_obj)
             
             # 2. Reformat to match Signal Processor expectations

scripts/test_shenron.py

@@ -164,11 +164,14 @@ def scan_convert_ra(ra_heatmap, range_axis, angle_axis, img_size=512):
     fov_mask = (Theta_query >= theta_min) & (Theta_query <= theta_max) & (R_query <= max_range)
 
     # 5. Map RA Heatmap to Cartesian Grid
-    # Calculate fractional indices
-    r_idx = np.clip(((R_query / max_range) * (ra_heatmap.shape[0] - 1)).astype(int), 0, ra_heatmap.shape[0] - 1)
-    # theta index: Shift by theta_min to align 0..120 range
-    theta_range = theta_max - theta_min
-    theta_idx = np.clip(((Theta_query - theta_min) / theta_range * (ra_heatmap.shape[1] - 1)).astype(int), 0, ra_heatmap.shape[1] - 1)
+    # Calculate fractional indices (Use epsilon to avoid div by zero warnings)
+    theta_range = max(theta_max - theta_min, 1e-9)
+    safe_max_range = max(max_range, 1e-9)
+    
+    with np.errstate(divide='ignore', invalid='ignore'):
+        r_idx = np.clip(((R_query / safe_max_range) * (ra_heatmap.shape[0] - 1)).astype(int), 0, ra_heatmap.shape[0] - 1)
+        # theta index: Shift by theta_min to align 0..120 range
+        theta_idx = np.clip(((Theta_query - theta_min) / theta_range * (ra_heatmap.shape[1] - 1)).astype(int), 0, ra_heatmap.shape[1] - 1)
 
     # Project
     cartesian = np.full((img_size, img_size), np.min(ra_heatmap), dtype=np.float64)
@@ -199,7 +202,7 @@ def run_testbench(iter_name):
         return
         
     iter_dir.mkdir(parents=True, exist_ok=True)
-    radar_types = ['awrl1432', 'radarbook']
+    radar_types = ['awrl1432', 'radarbook', '1843']
     
     print(f"\n======================================")
     print(f"SHENRON TESTBENCH ITERATION: {iter_name}")
@@ -214,11 +217,15 @@ def run_testbench(iter_name):
             models[r_type] = ShenronRadarModel(radar_type=r_type)
             (iter_dir / r_type).mkdir(exist_ok=True)
             
-            # Create Metrology folders
             met_base = iter_dir / r_type / "metrology"
             for sub in ["rd", "ra", "cfar"]:
                 (met_base / sub).mkdir(parents=True, exist_ok=True)
             
+            # Create ADC export folder if requested
+            adc_dir = iter_dir / r_type / "adc_raw"
+            adc_dir.mkdir(parents=True, exist_ok=True)
+            models[r_type].sim_config["adc_raw_dir"] = str(adc_dir)
+                
         except Exception as e:
             print(f"  -> [WARNING] Failed to init {r_type}: {e}")
             continue
@@ -240,6 +247,8 @@ def run_testbench(iter_name):
             
         for r_type, model in models.items():
             try:
+                frame_id = lidar_file.stem.split('_')[-1] # Extract number from frame_000XXX
+                model.sim_config["current_frame_id"] = frame_id
                 rich_pcd = model.process(data)
                 out_path = iter_dir / r_type / lidar_file.name
                 np.save(out_path, rich_pcd)