From c37f45e9b3dbff3049b5570150c7d0d8652ad28e Mon Sep 17 00:00:00 2001 From: rakadu1 Date: Mon, 13 Apr 2026 16:08:13 +0530 Subject: [PATCH] feat: Integrate 1843 radar profile with BPM support and ADC export --- .../ISOLATE/ConfigureRadar/ConfigureRadar.py | 162 ++++++++++ .../ConfigureRadar/heatmap_gen_fast.py | 299 ++++++++++++++++++ scripts/ISOLATE/ConfigureRadar/lidar.py | 242 ++++++++++++++ .../ConfigureRadar.py | 61 +++- .../lidar.py | 13 + .../shenron/heatmap_gen_fast.py | 65 +++- scripts/ISOLATE/model_wrapper.py | 5 + scripts/test_shenron.py | 23 +- 8 files changed, 847 insertions(+), 23 deletions(-) create mode 100644 scripts/ISOLATE/ConfigureRadar/ConfigureRadar.py create mode 100644 scripts/ISOLATE/ConfigureRadar/heatmap_gen_fast.py create mode 100644 scripts/ISOLATE/ConfigureRadar/lidar.py diff --git a/scripts/ISOLATE/ConfigureRadar/ConfigureRadar.py b/scripts/ISOLATE/ConfigureRadar/ConfigureRadar.py new file mode 100644 index 0000000..cb867df --- /dev/null +++ b/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 \ No newline at end of file diff --git a/scripts/ISOLATE/ConfigureRadar/heatmap_gen_fast.py b/scripts/ISOLATE/ConfigureRadar/heatmap_gen_fast.py new file mode 100644 index 0000000..99081bd --- /dev/null +++ b/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() + + diff --git a/scripts/ISOLATE/ConfigureRadar/lidar.py b/scripts/ISOLATE/ConfigureRadar/lidar.py new file mode 100644 index 0000000..1d3d656 --- /dev/null +++ b/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) 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 diff --git a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/lidar.py b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/lidar.py index 440dd3b..abe4439 100644 --- a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/lidar.py +++ b/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) 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 29cd0d7..81b53e6 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 @@ -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() diff --git a/scripts/ISOLATE/model_wrapper.py b/scripts/ISOLATE/model_wrapper.py index 8d43716..d52d535 100644 --- a/scripts/ISOLATE/model_wrapper.py +++ b/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 diff --git a/scripts/test_shenron.py b/scripts/test_shenron.py index a7490ef..36a6105 100644 --- a/scripts/test_shenron.py +++ b/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,10 +217,14 @@ 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}") @@ -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)