feat: Integrate C-SHENRON physics-based radar simulation pipeline

Comprehensive integration of the Shenron radar model, enabling high-fidelity
physics-driven radar synthesis from semantic LiDAR data.

Core Changes:
- Sensor Migration: Updated EGO vehicle to use 'sensor.lidar.ray_cast_semantic'.
- Recorder Enhancement: Modified src/recorder.py to dynamically capture
  6/7nd-column semantic LiDAR buffers [x, y, z, cos, obj, tag].
- Orchestration: Created scripts/ISOLATE/model_wrapper.py with the
  ShenronRadarModel class for a unified physics-to-detection API.
- Processor Logic: Fixed broadcasting bugs and hardcoded antenna counts
  in RadarProcessor to support dynamic radar models (radarbook/ti_cascade).
- Rich Data Output: Updated extraction logic to include [velocity, magnitude]
  parameters for ADAS performance analysis.
- Batch Pipeline: Added scripts/generate_shenron.py to synthesize radar
  data as a post-processing step, maintaining simulation performance.
- Visualization: Updated scripts/data_to_mcap.py to support the
  new /radar/shenron topic with a 5-field rich PointCloud schema.

Stability & Fixes:
- Implemented CUDA-enabled PyTorch checks with a robust CPU fallback.
- Corrected CARLA 0.9.16 semantic column mapping in ISOLATE/lidar.py.
- Handled indexing and range-slicing errors in the CFAR detection routine.

scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/.gitignore

@@ -0,0 +1,2 @@
+__pycache__
+.idea/

scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/ConfigureRadar.py

@@ -0,0 +1,105 @@
+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
+        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 == "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/e2e_agent_sem_lidar2shenron_package/README.md

@@ -0,0 +1,16 @@
+# SHENRON: Radar Simulation
+Packaging shenron into minimal working code
+
+To run the simulation, follow these steps:
+
+1. Open the `simulator_config.yaml` file.
+2. Add the file paths for the input and output files in the appropriate fields. For example:
+
+   ```yaml
+   PATHS :
+     LIDAR_PATH : "/home/Kshitiz/"
+     LIDAR_FOLDERS : ["semantic_lidar"]
+     OUT_PATH : "/home/Kshitiz/semantic_lidar/"
+3. Run the simulation using main file
+   ```python
+   python main.py

scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/carla_shenron_config.yaml

@@ -0,0 +1,2 @@
+INPUT_PATH : "/radar-imaging-dataset/Jason/mmfn/data1/nss_0702/Town04_short/"
+OUTPUT_PATH : "/radar-imaging-dataset/Jason/mmfn/data1/nss_0702/Town04_short/"

scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/lidar.py

@@ -0,0 +1,229 @@
+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
+    '''
+    # CARLA 0.9.16 [x, y, z, intensity, cos, obj, tag]
+    # We want [x, y, z, tag]
+    carla_sem_lidar_data_crop = carla_sem_lidar_data[:, (0, 1, 2, 6)]
+    temp_list = np.array([0, 4, 2, 0, 11, 5, 0, 0, 1, 8, 12, 3, 7, 10, 0, 1, 0, 12, 6, 0, 0, 0, 0])
+
+    col = temp_list[(carla_sem_lidar_data_crop[:, 3].astype(int))]
+    carla_sem_lidar_data_crop[:, 3] = col
+
+    return carla_sem_lidar_data_crop
+
+# 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"])
+    # 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)
+    # cos_inc_angle is at index 4 in [x, y, z, intensity, cos, obj, tag]
+    cosines = sem_lidar_frame[:, 4]
+    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)
+    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/lidar_utils.py

@@ -0,0 +1,216 @@
+import numpy as np
+import open3d as o3d
+import os
+import scipy.io as sio
+import sys
+sys.path.append("../")
+# from path_config import *
+import pdb
+
+def create_digital_twin(lidar_points, gt_labels):
+
+    """
+    Augments the input point cloud using the ground truth labels.
+    Could densify or replace dynamic objects with CAD
+    Input:
+        lidar_points: Nx6
+        gt_labels: GT boxes for the lidar frame
+
+    Output:
+        digital_twin: N'x6 Augmented point cloud
+    """
+    cad_id = 1
+    # pdb.set_trace()
+    # car_data = sio.loadmat(path_config["path_cad"] + f"car_cart{cad_id}.mat")
+    # car_cart = np.array(car_data['car_cart'])
+    
+    car_data = sio.loadmat(path_config["path_cad"] + f"chassis_camry.mat")
+    car_cart = np.array(car_data['chassis_loc'])
+
+    wheel_data = sio.loadmat(path_config["path_cad"] + f"wheel_camry.mat")
+    wheel_cart = np.array(wheel_data['wheel_loc'])
+
+    speed_multiplier = sio.loadmat(path_config["path_cad"] + f"speed_multiplier.mat")
+    speed_multiplier = np.array(speed_multiplier['mult_all'])
+
+    speed_multiplier = np.concatenate((np.ones(car_cart.shape[0],), np.squeeze(speed_multiplier)), axis=0)
+    car_cart = np.concatenate((car_cart, wheel_cart), axis=0)
+
+
+    veh_angle = gt_labels[-1]
+    rotMatrix = np.array([[np.cos(np.deg2rad(veh_angle)),
+                           np.sin(np.deg2rad(veh_angle))]
+                             , [- np.sin(np.deg2rad(veh_angle)),
+                                np.cos(np.deg2rad(veh_angle))]])
+            
+
+    car_cart[:, 0:2] = np.matmul(car_cart[:, 0:2], np.array([[0, -1], [1, 0]]))
+    gt_dims = gt_labels[3:6]
+    # dims_mat = sio.loadmat(path_config["path_cad"] + "dimensions.mat")
+    # dims_mat = dims_mat['dims']
+    # cad_dims = np.squeeze(dims_mat[dims_mat[:,3]==cad_id,:3]).tolist()
+
+    cad_dims = np.array([4.805,1.795,1.495])
+
+    
+    scale = gt_dims/cad_dims
+
+    new_car_cart = np.matmul(car_cart[:, 0:2], rotMatrix)
+
+    veh_x_new = gt_labels[1] + new_car_cart[:, 0]*scale[0]
+    veh_y_new = -gt_labels[0] + new_car_cart[:, 1]*scale[1]
+    veh_z_new = -1 + car_cart[:, 2]*scale[2]  # get the ground height
+    
+    ###calculate radial_speed
+       
+    speed = np.ones((veh_x_new.shape[0],2))*gt_labels[9:11]
+    # speed[points[:,5]==0.1,:] = gt_labels[9:11]
+    # x = -points[:,1]
+    # y = points[:,0]
+    x = -veh_y_new
+    y = veh_x_new
+
+    xy = np.hstack((x[:,None],y[:,None]))
+    rad_speed = np.sum((speed*xy)/np.linalg.norm(xy,axis=1)[:,None],1)
+    
+    veh_speed = rad_speed#*speed_multiplier
+
+    # veh_speed = np.zeros(np.size(new_car_cart[:, 0]))
+    veh_angle = np.zeros(np.size(new_car_cart[:, 0]))
+    veh_material = 0.1 * np.ones(np.size(new_car_cart[:, 0])) ## Car material
+    
+
+
+    car_pc = np.vstack((veh_x_new, veh_y_new, veh_z_new, veh_speed, veh_angle, veh_material)).T
+
+
+    
+
+
+    digital_twin = np.vstack((lidar_points,car_pc))
+
+    return digital_twin, veh_speed
+
+def map_material(test):
+    # test = test[test[:, 1] > 0, :]  # filter to obtain calibration points
+    # test = test[test[:, 1] < 10, :]
+    # test[test[:, 3] != 255, 3] = 5
+
+    
+    # amplify = 1
+    # after metal tuning - 0.01,0.1,0.001
+    conc = 0.03  # .04  # 0.3  # 2  # best  # calculated  # lucky
+    steel = 0.1  # 0.99  # 200
+    tree = 0.001  # 0.0001  # 0.01  # 0.02
+    human = 0.1 
+    test[test[:, 3] == 255, 3] = 0  # for unlabeled assign 0
+    test[test[:, 3] == 0, 3] = 0  # for road->concrete, 0 to suppress the ground rings that show up as objects
+    test[test[:, 3] == 1, 3] = conc  # for sidewalk->concrete
+    test[test[:, 3] == 2, 3] = conc  # for building->concrete
+    test[test[:, 3] == 3, 3] = conc  # for wall->concrete
+    test[test[:, 3] == 4, 3] = steel  # for fence->metal
+    test[test[:, 3] == 5, 3] = steel  # for pole->metal
+    test[test[:, 3] == 6, 3] = steel  # for traffic light->metal
+    test[test[:, 3] == 7, 3] = steel  # for traffic sign->metal
+    test[test[:, 3] == 8, 3] = tree  # for vegetation->trees  # just for wall data
+    test[test[:, 3] == 9, 3] = tree  # for terrain->trees  # just for wall data
+    test[test[:, 3] == 10, 3] = 0  # for sky->0
+    test[test[:, 3] == 11, 3] = human  # for person->metal (reflects significant energy, approx)
+    test[test[:, 3] == 12, 3] = steel  # for rider->metal
+    test[test[:, 3] == 13, 3] = steel  # for car->metal
+    test[test[:, 3] == 14, 3] = steel  # for truck->metal
+    test[test[:, 3] == 15, 3] = steel  # for bus->metal
+    test[test[:, 3] == 16, 3] = steel  # for train->metal
+    test[test[:, 3] == 17, 3] = steel  # for motorbike->metal
+    test[test[:, 3] == 18, 3] = steel  # for bicycle->metal
+
+    return test
+
+def new_map_material(test):
+    # map material to index 
+    #0 -> unlabeled
+    #1 -> wood
+    #2 -> concrete
+    #3 -> human
+    #4 -> metal
+    
+    unlabel = 0
+    wood = 1
+    conc = 2
+    human = 3 
+    metal = 4
+
+    unlabel_roughness = 0
+    wood_roughness = 0.0017 #1.7mm
+    conc_roughness = 0.0017 #1.7mm
+    human_roughness = 0.0001 # 100um
+    metal_roughness = 0.0001 # 100um 
+    
+    roughness = np.array([unlabel_roughness,wood_roughness,conc_roughness,human_roughness,metal_roughness])
+
+    unlabel_perm = 1
+    wood_perm = 2 
+    conc_perm = 5.24 
+    human_perm = 15 
+    metal_perm = 100000
+    
+    permittivity = np.array([unlabel_perm,wood_perm,conc_perm,human_perm,metal_perm])
+
+
+    test[test[:, 3] == 255, 3] = unlabel  # for unlabeled assign 0
+    test[test[:, 3] == 0, 3] = unlabel  # for road->unlabeled, to suppress the ground rings that show up as objects
+    test[test[:, 3] == 1, 3] = conc  # for sidewalk->concrete
+    test[test[:, 3] == 2, 3] = conc  # for building->concrete
+    test[test[:, 3] == 3, 3] = conc  # for wall->concrete
+    test[test[:, 3] == 4, 3] = metal  # for fence->metal
+    test[test[:, 3] == 5, 3] = metal  # for pole->metal
+    test[test[:, 3] == 6, 3] = metal  # for traffic light->metal
+    test[test[:, 3] == 7, 3] = metal  # for traffic sign->metal
+    test[test[:, 3] == 8, 3] = wood  # for vegetation->trees  # just for wall data
+    test[test[:, 3] == 9, 3] = wood  # for terrain->trees  # just for wall data
+    test[test[:, 3] == 10, 3] = unlabel  # for sky->0
+    test[test[:, 3] == 11, 3] = human  # for person->metal (reflects significant energy, approx)
+    test[test[:, 3] == 12, 3] = metal  # for rider->metal
+    test[test[:, 3] == 13, 3] = metal  # for car->metal
+    test[test[:, 3] == 14, 3] = metal  # for truck->metal
+    test[test[:, 3] == 15, 3] = metal  # for bus->metal
+    test[test[:, 3] == 16, 3] = metal  # for train->metal
+    test[test[:, 3] == 17, 3] = metal  # for motorbike->metal
+    test[test[:, 3] == 18, 3] = metal  # for bicycle->metal
+    
+    return test
+
+if __name__ == '__main__':
+
+    gt = sio.loadmat("../gt_11_08_car_round2.mat")
+    gt = gt["a"]
+    
+
+    # print(gt_label)
+    exp_date = "11_08"
+    lidar_path = f"Y:/media/Kshitiz/RADARIMAGING/radar_simulator_data/{exp_date}_data/{exp_date}_lidar_appended"
+    lidar_folder = f"test_11_08_car_round2"
+    # file = "0001.npy"
+    files = np.array(os.listdir(f"{lidar_path}/{lidar_folder}/"))
+
+    select_array = np.arange(50,51)
+    for file_idx, file in enumerate(files[select_array]):
+        lidar_file_path = os.path.join(f"{lidar_path}/{lidar_folder}/", file)
+        
+        test = np.load(lidar_file_path)
+        test = map_material(test)
+            
+        points = np.zeros((np.shape(test)[0], 6))
+        
+        points[:, [0, 1, 2]] = test[:, [0, 2, 1]]
+        
+        points[:, 5] = test[:, 3]
+
+        gt_label = gt[select_array[file_idx],:]
+        points = create_digital_twin(points, gt_label)
+
+        pcd = o3d.geometry.PointCloud()
+        pcd.points = o3d.utility.Vector3dVector(points[:, 0:3])
+        o3d.visualization.draw_geometries([pcd])
+
+    

scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/main.py

@@ -0,0 +1,63 @@
+# -*- coding: utf-8 -*-
+"""
+Created on Wed Apr  7 16:00:26 2021
+
+@author: ksban
+"""
+
+from lidar import run_lidar
+import os
+import yaml
+
+def run_shenron(sim_config, base_folder):
+    print("in run shenron")
+    # with open('simulator_configs.yaml', 'r') as f:
+    #     sim_config = yaml.safe_load(f)
+    
+    # printing the base path here for the check
+    # print(f"the base folder currently is base/")
+
+    folders = os.listdir(f"{sim_config['BASE_PATH']}/{base_folder}")
+    folders.sort()
+    # print(folders)
+    # run_folders = ["set4_vertical_crossing_2023-10-08-13-07-48", "set3_split_vertical_crossing_2023-10-08-12-56-58", "set2_vertical_crossing_2023-10-08-12-24-52", "set1_vertical_crossing_2023-10-08-12-02-41"]
+    # return
+    # run_folders= ["Town01_20_08_10_09_47_16"] 
+    # run_folders= ["Town01_10_08_10_06_43_59"] #with 3 chirps only
+    for folder in folders:
+        # if folder in run_folders:
+        if os.path.isdir(f'{sim_config["BASE_PATH"]}/{base_folder}/{folder}/'):
+            exec_path = f'{sim_config["BASE_PATH"]}/{base_folder}/{folder}/'
+            print(f"currently running the folder {exec_path.split('/')[-2]}")
+            if sim_config["INPUT"] == "lidar":
+                run_lidar(sim_config, exec_path)
+            else:
+                print("Incorrect input in config")
+            
+            print("this folder done")
+
+        else:
+            print("skipping this, not a folder")
+            continue
+        
+        # else:
+        #     continue
+        
+if __name__ == '__main__':
+
+    with open('simulator_configs.yaml', 'r') as f:
+        sim_config = yaml.safe_load(f)
+
+    # if sim_config["INPUT"] == "lidar":
+    #     run_lidar(sim_config)
+    # else:
+    #     print("Incorrect input in config")
+    print("in main")
+
+    base_base_folders = os.listdir(sim_config["BASE_PATH"])
+    base_base_folders.sort()
+    run_folders = ["Town05_tiny", "Town06_short", "Town06_tiny", "Town07_short", "Town07_tiny", "Town10_short", "Town10_tiny",]
+    for base_folder in base_base_folders:
+        if base_folder in run_folders:
+            print(f"currently running the {base_folder}")
+            run_shenron(sim_config, base_folder)

scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/map_carla_semantic_lidar_shenron.py

@@ -0,0 +1,63 @@
+import numpy as np
+import os
+import yaml
+# import run_shenron
+# from main import run_shenron
+
+def check_save_path(save_path):
+    if not os.path.exists(save_path):
+        os.makedirs(save_path)
+    return
+
+def main():
+    
+    with open('carla_shenron_config.yaml', 'r') as f:
+        config = yaml.safe_load(f)
+    
+    in_path = config["INPUT_PATH"]
+    out_path = config["OUTPUT_PATH"]
+
+    if not os.path.exists(out_path):
+        os.makedirs(out_path, exist_ok=True)
+
+    folders = os.listdir(in_path)
+    folders.sort()
+    print(folders)
+    print("folders loaded, mapping now")
+
+    for folder in folders:
+        if os.path.isdir(f'{in_path}/{folder}'):
+            print(f"starting {folder}")
+            out_path_temp = f'{out_path}/{folder}/carla_shenron_sem_lidar'
+            check_save_path(out_path_temp)
+            
+            files = os.listdir(f'{in_path}/{folder}/semantic_lidar/')
+            files.sort()
+
+            for file in files:
+                carla_sem_lidar_data = np.load(f'{in_path}/{folder}/semantic_lidar/{file}')
+                carla_sem_lidar_data = carla_sem_lidar_data[:, (0, 1, 2, 5)]
+                carla_sem_lidar_data[:, 3] = carla_sem_lidar_data[:, 3]-1
+                
+                carla_sem_lidar_data[carla_sem_lidar_data[:, 3]>18, 3] = 255.
+                carla_sem_lidar_data[carla_sem_lidar_data[:, 3]<0, 3] = 255.
+                # print(carla_sem_lidar_data)
+                carla_sem_lidar_data[:, (0, 1, 2)] = carla_sem_lidar_data[:, (0, 2, 1)]
+                # print(carla_sem_lidar_data)
+                # break
+                # print(np.unique(carla_sem_lidar_data[:, 3]))
+                # break
+                np.save(f'{out_path_temp}/{file[:-4]}.npy', carla_sem_lidar_data)
+            print(f"{folder} done")
+        else:
+            print("skipping this, not a directory")
+            continue
+
+    #call shenron here after saving carla_shenron_semantic_lidar for all the folders
+    # print("runnning shenron now")
+    # run_shenron()
+
+
+if __name__ == "__main__":
+    print("starting map_carla_semantic_lidar_shenron")
+    main()

scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/path_config.py

@@ -0,0 +1,43 @@
+# class path_config:
+# 	path_csv = "/media/ehdd_8t1/RadarImaging/collab_radar_data/simulator_inputs/sumo_traffic_data/"
+# 	path_ply = "/media/ehdd_8t1/RadarImaging/collab_radar_data/simulator_inputs/point_cloud_maps/"
+# 	path_cad = "/media/ehdd_8t1/RadarImaging/collab_radar_data/simulator_inputs/car_cad_models/"
+
+path_config = {
+    "path_csv": "/media/ehdd_8t1/RadarImaging/collab_radar_data/simulator_inputs/sumo_traffic_data/",
+    "path_ply": "/media/ehdd_8t1/RadarImaging/collab_radar_data/simulator_inputs/point_cloud_maps/",
+    "path_cad": "/media/ehdd_8t1/RadarImaging/collab_radar_data/simulator_inputs/car_cad_models/",
+    "path_ped": "/media/ehdd_8t1/RadarImaging/collab_radar_data/simulator_inputs/human_cad_models/"
+    }
+
+out_paths = {
+    "path_tensors": "/media/ehdd_8t1/RadarImaging/collab_radar_data/radar_tensors/",
+    "path_gt": "/media/ehdd_8t1/RadarImaging/collab_radar_data/ground_truth/",
+    "path_images": "/media/ehdd_8t1/RadarImaging/collab_radar_data/radar_bev_images/"
+    }
+
+dataset_config = {
+	"dataset_name": "downtown_SD_10thru_50count", #"ucsd_atkinson_1"
+	"ply_name": "downtown_SD_10_7.ply", #"ucsd_atkinson_10_7.ply"
+	"csv_name": "downtown_SD_10thru_50count_10Hz_with_cad_id"
+}
+
+ped_dataset_config = {
+	"dataset_name": "test_11_08_person3", 
+	"npy_name": "test_11_08_person3_segmented", 
+}
+
+out_config = {
+	"save_dataset_name" : "downtown_SD_10thru_50count_1deg_10Hz"
+}
+
+debug_config = {
+	"save_cropped_path" : "/media/ehdd_8t1/RadarImaging/collab_radar_data/debug/"
+}
+
+# class data_paths:
+# 	path_tensors = "/media/ehdd_8t1/RadarImaging/collab_radar_data/radar_tensors/"
+# 	path_gt = "/media/ehdd_8t1/RadarImaging/collab_radar_data/ground_truth/"
+# 	path_images = "/media/ehdd_8t1/RadarImaging/collab_radar_data/images_no_white/"
+
+

scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/shenron/Sceneset.py

@@ -0,0 +1,546 @@
+from re import A
+import open3d as o3d
+import os
+import numpy as np
+import math
+import matplotlib.pyplot as plt
+import pdb
+from e2e_agent_sem_lidar2shenron_package.ConfigureRadar import radar
+import sys
+
+def cart2sph(x, y, z):
+    hxy = np.hypot(x, y)
+    r = np.hypot(hxy, z)
+    el = np.arctan2(z, hxy)
+    az = np.arctan2(y, x)
+    return az, el, r
+
+def sph2cart( az, el, r):
+    rcos_theta = r * np.cos(el)
+    x = rcos_theta * np.cos(az)
+    y = rcos_theta * np.sin(az)
+    z = r * np.sin(el)
+    return x, y, z
+
+class Sceneset():
+
+    """
+    Class with all the functions to modify OSM scenario and thus generate the points for radar heatmap
+    """
+    def __init__(self, pc):
+        self.points_3D = pc
+        self.rad_scene = self.points_3D
+        pass
+
+    def removeocclusion(self, radar):
+
+        row = np.where(self.points_3D.any(axis=1))[0]
+        self.points_3D = self.points_3D[row, :]
+
+        # to reduce volume to 100x150x25, else voxel filter too big
+        rowx = np.where((self.points_3D[:, 0] < 50) & (self.points_3D[:, 0] > -50))  # x limits
+        self.points_3D = self.points_3D[rowx, :].squeeze(0)
+        rowy = np.where((self.points_3D[:, 1] < 150) & (self.points_3D[:, 1] > 0))  # y limits
+        self.points_3D = self.points_3D[rowy,:].squeeze(0)
+        rowz = np.where((self.points_3D[:, 2] < 5) & (self.points_3D[:, 2] > -2)) #z limits
+        self.points_3D = self.points_3D[rowz,:].squeeze(0)
+        rowx = []
+        rowy = []
+        rowz = []
+
+        self.rad_scene = self.points_3D
+        sph_v = np.zeros((self.points_3D.shape[0], 3))
+        sph_v[:, 0], sph_v[:, 1], sph_v[:, 2] = cart2sph(self.points_3D[:, 0]-radar.center[0], self.points_3D[:, 1]-radar.center[1], self.points_3D[:, 2]-radar.elv)
+        sphlim = np.array([np.min(sph_v, axis=0), np.max(sph_v, axis=0)]) # limits in all three dimensions in the spherical coordinates
+
+
+        pcd2 = o3d.geometry.PointCloud()
+        pcd2.points = o3d.utility.Vector3dVector(self.rad_scene[:, 0:3])
+        # o3d.visualization.draw_geometries([pcd2])
+        
+
+        # convert the point cloud into a grid of 0s and 1s
+        # define voxel size of the grid
+        phi_res = radar.voxel_phi / 180 * math.pi
+        theta_res = radar.voxel_theta / 180 * math.pi
+        rho_res = radar.voxel_rho
+
+        sph_m_phi = np.arange(sphlim[0, 0], sphlim[1, 0] + phi_res,phi_res) + phi_res
+        sph_m_theta = np.arange( sphlim[0, 1], sphlim[1, 1] + theta_res, theta_res) + theta_res
+        sph_m_rho = np.arange(sphlim[0, 2], sphlim[1, 2] + rho_res, rho_res) + rho_res
+        sph_m_size = [len(sph_m_phi), len(sph_m_theta), len(sph_m_rho)]
+        sph_m = np.zeros(sph_m_size)
+        vel_m = np.zeros(sph_m_size)
+        mtl_m = np.zeros(sph_m_size)
+
+        phi_m_idx = np.round_((sph_v[:, 0] - sphlim[0, 0]) / phi_res)  # +1
+        theta_m_idx = np.round_((sph_v[:, 1] - sphlim[0, 1]) / theta_res)  # +1
+        rho_m_idx = np.round_((sph_v[:, 2] - sphlim[0, 2]) / rho_res)  # +1
+        for k_pt in range(0, len(self.points_3D)):
+            sph_m[int(phi_m_idx[k_pt]), int(theta_m_idx[k_pt]), int(rho_m_idx[k_pt])] = 1
+            vel_m[int(phi_m_idx[k_pt]), int(theta_m_idx[k_pt]), int(rho_m_idx[k_pt])] = self.points_3D[k_pt, 3]
+            mtl_m[int(phi_m_idx[k_pt]), int(theta_m_idx[k_pt]), int(rho_m_idx[k_pt])] = self.points_3D[k_pt, 4]
+
+        visible_sph_m = np.zeros(sph_m.shape)
+        visible_vel_m = np.zeros(vel_m.shape)
+        visible_mtl_m = np.zeros(mtl_m.shape)
+        for kphi in range(0, sph_m_size[0]):
+            for ktheta in range(0, sph_m_size[1]):
+                if sph_m[kphi, ktheta, :].any() > 0:
+                    krho = np.where(sph_m[kphi, ktheta, :] > 0)[0][0]
+                    visible_sph_m[kphi, ktheta, krho] = sph_m[kphi, ktheta, krho]
+                    visible_vel_m[kphi, ktheta, krho] = vel_m[kphi, ktheta, krho]
+                    visible_mtl_m[kphi, ktheta, krho] = mtl_m[kphi, ktheta, krho]
+
+        visible_sph_m_idx = np.where(visible_sph_m)
+        # sph_v_idx = []
+        sph_v_idx = np.array([visible_sph_m_idx[0], visible_sph_m_idx[1], visible_sph_m_idx[2]]).T  # np.unravel_index( visible_sph_m_idx, sph_m_size)
+        visible_vel_v = visible_vel_m[visible_sph_m_idx]
+        visible_mtl_v = visible_mtl_m[visible_sph_m_idx]
+        visible_sph_v = np.array([sph_m_phi[sph_v_idx[:, 0]], sph_m_theta[sph_v_idx[:, 1]], sph_m_rho[sph_v_idx[:, 2]]]).T
+
+        visible_cart_v = np.zeros(visible_sph_v.shape)
+        [visible_cart_v[:, 0], visible_cart_v[:, 1], visible_cart_v[:, 2]] = sph2cart(visible_sph_v[:, 0], visible_sph_v[:, 1], visible_sph_v[:, 2])
+        self.rad_scene = np.vstack((visible_cart_v[:, :].T, visible_vel_v, visible_mtl_v)).T+np.hstack((radar.center, radar.elv, np.array([0, 0])))
+        
+
+        # pcd2 = o3d.geometry.PointCloud()
+        # pcd2.points = o3d.utility.Vector3dVector(self.rad_scene[:, 0:3])
+        
+        # o3d.visualization.draw_geometries([pcd2])
+        
+
+        # ground_refl = np.vstack((visible_cart_v[:, :].T, visible_vel_v, visible_mtl_v)).T+np.array(radar.center + [1] + [0, 0])
+        # self.rad_scene = np.append(self.rad_scene, ground_refl*np.array([1, 1, -1, 1, 1]), axis=0)
+
+    def removeocclusion_hiddenpoint(self, radar):
+        row = np.where(self.points_3D.any(axis=1))[0]
+        self.points_3D = self.points_3D[row, :]
+
+        # to reduce volume to 100x150x25, else voxel filter too big
+        rowx = np.where((self.points_3D[:, 0] < 50) & (self.points_3D[:, 0] > -50))  # x limits
+        self.points_3D = self.points_3D[rowx, :].squeeze(0)
+        rowy = np.where((self.points_3D[:, 1] < 150) & (self.points_3D[:, 1] > 0))  # y limits
+        self.points_3D = self.points_3D[rowy,:].squeeze(0)
+        rowz = np.where((self.points_3D[:, 2] < 5) & (self.points_3D[:, 2] > -2)) #z limits
+        self.points_3D = self.points_3D[rowz,:].squeeze(0)
+        rowx = []
+        rowy = []
+        rowz = []
+
+        self.rad_scene = self.points_3D
+        vel = self.points_3D[:,3]
+        mtl = self.points_3D[:,4]
+
+        pcd = o3d.geometry.PointCloud()
+        pcd.points = o3d.utility.Vector3dVector(self.rad_scene[:, 0:3])
+        # o3d.visualization.draw_geometries([pcd])
+
+
+        diameter = np.linalg.norm(
+            np.asarray(pcd.get_max_bound()) - np.asarray(pcd.get_min_bound()))
+
+        print("Define parameters used for hidden_point_removal",diameter)
+        camera = [radar.center[0], radar.center[1], radar.elv]
+        radius = 10000
+        # pdb.set_trace()
+        print("Get all points that are visible from given view point")
+        try:
+            _, pt_map = pcd.hidden_point_removal(camera, radius)
+        except Exception as e:
+            pt_map = np.arange(self.points_3D.shape[0])
+            print(e)
+
+        
+        self.rad_scene = np.hstack((np.array(pcd.points)[pt_map],vel[pt_map,None],mtl[pt_map,None]))
+        
+        print(f"Number of points in point cloud = {self.rad_scene.shape[0]}")
+        vel = self.rad_scene[:,3]
+        mtl = self.rad_scene[:,4]
+        pcd = o3d.geometry.PointCloud()
+        pcd.points = o3d.utility.Vector3dVector(self.rad_scene[:, 0:3])
+        pcd_tree = o3d.geometry.KDTreeFlann(pcd)
+        downpcd = pcd.voxel_down_sample(voxel_size=0.05)
+        valid_points= np.zeros((1,5))
+        for point in range(0,len(np.asarray(downpcd.points))):
+            [_, idx,_] = pcd_tree.search_knn_vector_3d(downpcd.points[point], 1)
+            valid_point = np.concatenate((np.array(pcd.points)[idx].T,vel[idx,None],mtl[idx,None]),axis=0).flatten()
+            valid_points = np.concatenate((valid_points,[valid_point]),axis=0)
+        valid_points=np.delete(valid_points,0,axis=0)    
+        self.rad_scene = valid_points
+        print(f"Number of points after downsampling = {self.rad_scene.shape[0]}")
+
+        # print(f"Number of points in point cloud = {self.rad_scene.shape[0]}")
+        # # downsample_ind = np.random.choice(self.rad_scene.shape[0], round(0.25*self.rad_scene.shape[0]))
+        # try:
+        #     downsample_ind = np.random.choice(self.rad_scene.shape[0], 4000) ## need to fix this by breaking the number of points in heatmapgen
+        #     self.rad_scene = self.rad_scene[downsample_ind,:]
+        # except Exception as e:
+        #     pass
+
+
+        
+        # print(f"Number of points after downsampling = {self.rad_scene.shape[0]}")
+
+        # pcd2 = o3d.geometry.PointCloud()
+        # pcd2.points = o3d.utility.Vector3dVector(self.rad_scene[:, 0:3])
+        # o3d.visualization.draw_geometries([pcd2])
+        # sys.exit()
+
+    def specularpoints(self, radar):
+
+        pcd = o3d.geometry.PointCloud()
+        pcd.points = o3d.utility.Vector3dVector(self.rad_scene[:, 0:3])
+
+        pcd.estimate_normals( search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=10, max_nn=10))
+        # o3d.visualization.draw_geometries([pcd])
+
+        # plt.figure()
+        # pcd = o3d.geometry.PointCloud()
+        # pcd.points = o3d.utility.Vector3dVector(self.rad_scene)
+        # o3d.visualization.draw_geometries([pcd])
+        # plt.title('Estimated Normals of Point Cloud')
+
+        x = np.array(pcd.points)[:, 0]
+        y = np.array(pcd.points)[:, 1]
+        z = np.array(pcd.points)[:, 2]
+        u = np.array(pcd.normals)[:, 0]
+        v = np.array(pcd.normals)[:, 1]
+        w = np.array(pcd.normals)[:, 2]
+
+        # fig = plt.figure()
+        # ax = fig.gca(projection='3d')
+        # ax.quiver(x, y, z, u, v, w)
+        # plt.show()
+        
+        angles = np.arccos(np.sum(np.array(pcd.normals)[:, 0: 3]*((np.array([x, y, z])-np.hstack((radar.center, radar.elv)).reshape((3, 1))).T /np.linalg.norm(np.array([x, y, z])-np.hstack((radar.center, radar.elv)).reshape((3, 1)), axis=0)[:, np.newaxis]), axis=1))
+        angles_carla = self.rad_scene[:, 5]
+        spec_mask = self.rad_scene
+
+        rho = np.linalg.norm(spec_mask[:, 0:3] - np.hstack((radar.center, radar.elv)), axis=1)
+        theta = math.pi / 2 - np.arctan(((spec_mask[:, 0] - radar.center[0]) / (spec_mask[:, 1] - radar.center[1])))
+        elev_angle = np.arccos((spec_mask[:, 2] - radar.elv)/rho)
+        # loss = np.zeros((len(theta),), dtype=float)
+        # for i in range(len(theta)):
+        #     if self.rad_scene[i, 4] == 0.1:
+        #         loss[i] = (0.02 * (((1 + np.abs(np.cos(angles[i]))) / 2) ** 0.01) + 0.15*np.abs(np.cos(angles[i]))**300) * self.rad_scene[i, 4] * rho[i] * rho[i] * 3282 * math.radians(radar.voxel_phi) * math.radians(radar.voxel_theta)  # + 0.1*np.abs(np.cos(angles))**300
+        #     else:  # after metal without concrete 0.15,300 # after concrete 0.6,600 (just for the else loss)
+        #         loss[i] = (0.02 * (((1 + np.abs(np.cos(angles[i]))) / 2) ** 0.01) + 0.6*np.abs(np.cos(angles[i]))**600) * self.rad_scene[i, 4] * rho[i] * rho[i] * 3282 * math.radians(radar.voxel_phi) * math.radians(radar.voxel_theta)  # radar.voxel_phi * radar.voxel_theta
+        speed = self.rad_scene[:, 3]  # 0.05&5 , 0.1&300
+        
+
+
+        # file_path = os.path.join('F:/28_07_data/28_07_lidar_data/test_28_7_run11/mtlb-pcdFromBag', '0001.pcd')
+        # pc = o3d.io.read_point_cloud(file_path)
+        # pc.points = o3d.utility.Vector3dVector(self.rad_scene[:, 0:3])
+        # o3d.visualization.draw_geometries([pc])
+        loss_att,_,_ = get_loss_3(self.rad_scene, rho, elev_angle , angles_carla, radar, use_spec = False, use_diffused = True, no_material=False)
+
+        return rho, theta, loss_att, speed, angles
+
+
+def get_loss(points, rho, angles, radar, use_spec = True, use_diffused = True, no_material = False):
+    
+    # loss = np.zeros((len(theta),), dtype=float)
+    
+    if no_material:
+        points[:,4] = 1
+
+    constant = points[:, 4] * rho * rho * 3282 * math.radians(radar.voxel_phi) * math.radians(radar.voxel_theta) # N-points length
+    
+    diff_scatter = 0.02 * (((1 + np.abs(np.cos(angles))) / 2) ** 0.01) # N-points length
+
+    spec_gain = 0.6 * np.ones(len(angles),)
+    spec_exp = 600 * np.ones(len(angles),)
+
+    #for metal
+    spec_gain[points[:, 4] == 0.1] = 0.15
+    spec_exp[points[:, 4] == 0.1] = 300
+
+    specular = spec_gain*np.abs(np.cos(angles))**spec_exp
+
+    loss = (use_diffused * diff_scatter + use_spec * specular) * constant
+
+    return loss, diff_scatter*constant, specular*constant
+
+
+def get_loss_2(points, rho, angles, radar, use_spec = True, use_diffused = True, no_material = False):
+    '''
+    Note: angles should be used for power calculation as it is in reflecting surface coordinate system while theta is in radar coordinate system
+
+    Implemement power conservation here:
+    P_inc = P_return + P_trans
+    P_inc = P_inc * T^2 + P_inc * (1 - T^2)
+    P_inc = P_inc * T^2 (R^2 + S^2) + P_trans
+
+    Assume P_trans=0 or T=1. This can be more accurately calculated from fresnel equations
+    
+    P_inc = (P_inc * R^2) + (P_inc * S^2)
+    P_inc = P_spec + P_scat
+    '''
+    
+    if no_material:
+        points[:,4] = 1
+
+
+    ##Assuming rho and theta are calculated correctly
+    P_spec_inc = np.zeros((len(angles),), dtype=float)
+    P_scat_inc = np.zeros((len(angles),), dtype=float)
+
+    K_sq = 3282 # Gt Pt 
+    voxel_theta = np.deg2rad(radar.voxel_theta)
+    voxel_phi = np.deg2rad(radar.voxel_phi)
+
+    tx_dist_loss_exponent = 0
+    rx_dist_loss_exponent = 0
+
+    P_incident = np.power(rho,2) * np.sin(voxel_theta) * voxel_phi * voxel_theta * (1/np.power(rho,tx_dist_loss_exponent)) * K_sq # rho.^1.4
+
+    
+
+    #Material dependent qualtities
+    if use_spec and use_diffused:
+        R_sq = np.ones((len(angles),))*0.8 # metal 0.3
+        R_sq[points[:,4]==0.03] = 0.2 # Concrete 
+        R_sq[points[:,4]==0.001] = 0.2 # Vegetation 
+        R_sq[points[:,4]==0] = 0.1 # Road
+        S_sq = 1 - R_sq # Scattering coefficient
+
+    elif not use_spec:
+        R_sq = np.zeros((len(angles),))
+        S_sq = 1 - R_sq # Scattering coefficient
+
+    elif not use_diffused:
+        R_sq = np.ones((len(angles),))
+        S_sq = 1 - R_sq # Scattering coefficient
+
+    else:
+        print("Both Scatter and specular can't be false")
+        AssertionError
+
+    P_absorbed_fac = np.zeros((len(angles),)) # metal
+    P_absorbed_fac[points[:,4]==0.03] = 0.8 # concrete
+    P_absorbed_fac[points[:,4]==0.001] = 0.8 # vegetation
+    P_absorbed_fac[points[:,4]==0] = 1 # road
+    
+
+    P_reflected = P_incident*(1 - P_absorbed_fac)
+
+    spec_exp = np.ones((len(angles),))*600
+    spec_exp[points[:,4]==0.1] = 200 #300 # metal
+    
+    normalization = np.ones((len(angles),))*(1/0.10229)
+    normalization[points[:,4]==0.1] = (1/0.17702) # metal
+    
+
+
+    P_spec = P_reflected * R_sq
+    P_scat = P_reflected * S_sq
+
+    ##find power in the incident direction now
+    # pdb.set_trace()
+    # P_spec_inc[points[:,4]==0.1] = points[points[:,4]==0.1,4]*0.15*np.abs(np.cos(angles[points[:,4]==0.1]))**300
+    # P_spec_inc[points[:,4]!=0.1] = points[points[:,4]!=0.1,4]*0.6*np.abs(np.cos(angles[points[:,4]!=0.1]))**600
+
+    P_spec_inc = normalization*(np.abs(np.cos(angles))**spec_exp)*(1/np.power(rho,rx_dist_loss_exponent))
+    # P_spec_inc[points[:,4]!=0.1] = (1/0.10229)*(np.abs(np.cos(angles[points[:,4]==0.1]))**600)*(1/np.power(rho,2))
+
+    P_spec_inc = P_spec_inc * P_spec
+
+    #Lambertian
+    P_scat_inc = P_scat* ((0.5*(1+abs(np.cos(angles))))**0.1)*(1/3.07385) * (1/np.power(rho,rx_dist_loss_exponent)) # lambda sqaure aperture taken care in heatmap
+    
+    P_spec_inc = P_spec_inc * 10e2 #30e2 #0.8e2
+    P_scat_inc = P_scat_inc * 10e2 #30e2
+
+    loss = P_spec_inc + P_scat_inc
+
+    return loss, P_scat_inc, P_spec_inc
+
+def get_loss_3(points, rho, elev_angle, angles, radar, use_spec = True, use_diffused = True, no_material = False):
+    '''
+    Note: angles should be used for power calculation as it is in reflecting surface coordinate system while theta is in radar coordinate system
+
+    Implemement power conservation here:
+    P_inc = P_return(rough_loss^2 + S^2) + P_trans
+    P_scatter = P_return*S^2
+    P_return = P_inc * R^2  (R -> reflection co-eff from fresnel equations)
+
+    S = sqrt(1 - rough_loss^2)
+
+    
+    R_TE = cos(incd_angle) - sqrt(perm - sin(incd_angle)^2) / cos(incd_angle) + sqrt(perm - sin(incd_angle)^2)
+    R_TM = perm*cos(incd_angle) - sqrt(perm - sin(incd_angle)^2) / perm*cos(incd_angle) + sqrt(perm - sin(incd_angle)^2)
+    R^2 = (R_TE^2 + R_TM^2)/2
+
+    Scatter_loss = R^2 * S^2 = R^2 * (1- rough_loss^2)
+    rough_loss = exp(-0.5((4*pi*std_rough*cos(inc_angle)/lambda)^2))
+
+    backscatter_lobe_loss = E_so^2 * (lobe_frac*cos(incd_angle)^2*alpha_R + (1-lobe_frac))
+    '''
+
+    # standard deviation of surface roughness
+    unlabel_roughness = 0
+    wood_roughness = 0.0017 #1.7mm
+    conc_roughness = 0.0017 #1.7mm
+    human_roughness = 0.01 # 100um
+    metal_roughness = 0.00005 # 100um 
+    
+    roughness = np.array([unlabel_roughness,wood_roughness,conc_roughness,human_roughness,metal_roughness])
+
+    unlabel_perm = 1
+    wood_perm = 2 
+    conc_perm = 5.24 
+    human_perm = 1#15 
+    metal_perm = 100000
+    
+    permittivity = np.array([unlabel_perm,wood_perm,conc_perm,human_perm,metal_perm])
+
+    scat_normalization = 1/1.09*np.ones((len(angles),)) #*(1/0.10229) #####?????????#### this should bring the Eso^2 #####?????????####
+    lobe_frac = 0.9 # ratio of energy in main_lobe/back_scatter_lobe
+    spec_lobe_exp = 4.0 #lobe exponential along specular direction
+
+    K_sq = 3282 # Gt Pt #####?????????#### is this from the peak amplitude calculation #####?????????####
+    voxel_theta = np.deg2rad(radar.voxel_theta)
+    voxel_phi = np.deg2rad(radar.voxel_phi)
+
+    tx_dist_loss_exponent = 2
+    rx_dist_loss_exponent = 0
+
+    spec_angle_thresh = 2*np.pi/180#*(1/rho)
+
+    P_incident = np.power(rho,2) * np.sin(elev_angle) * voxel_phi * voxel_theta * (1/np.power(rho,tx_dist_loss_exponent)) * K_sq 
+
+    material = np.array(points[:,4])
+    material = np.asarray(material, dtype = 'int')
+    # material[:] = 3
+
+    #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_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])}")
+
+    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 = 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
+
+    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)
+        # 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_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))
+
+        # 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 = P_scat * np.power(P_scat_lobe,2) #lobe shape is for amplitude, need to square for power
+
+        P_spec = np.zeros((len(angles),))
+
+    elif not 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_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))
+
+        # 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 = P_scat * np.power(P_scat_lobe,2) #lobe shape is for amplitude, need to square for power
+
+        P_scat = np.zeros((len(angles),))
+
+        # P_spec = P_reflected * rough_sq 
+        # P_spec = P_spec * np.ones((len(angles),)) * (abs(angles) < 2*np.pi/180) #####?????????#### specular reflection only if angle < 2 degrees #####?????????####
+
+    elif not use_spec and not use_diffused:
+        print("Both Scatter and specular can't be false")
+        AssertionError
+
+    P_spec = P_spec *4*100*25/9
+    P_scat = P_scat *100*25/9
+
+    loss = P_spec + P_scat
+    # loss = loss * 2**7.2
+
+    # fig, ax = plt.subplots(1,7,  figsize=(10,5))
+    # ax[0].plot(loss)
+    # ax[0].set_title("total loss",fontsize=10)
+    # ax[1].plot(P_spec)
+    # ax[1].set_title("specular loss",fontsize=10)
+    # ax[2].plot(P_scat)
+    # ax[2].set_title("scatter loss",fontsize=10)
+    # ax[3].plot(material)
+    # ax[3].set_title("material",fontsize=10)
+    # ax[4].plot(angles)
+    # ax[4].set_title("angles",fontsize=10)
+    # ax[5].plot(R_TE_sq)
+    # ax[5].set_title("R TE",fontsize=10)
+    # ax[6].plot(R_TM_sq) #abs(numerator)/abs(denominator))
+    # ax[6].set_title("R TM",fontsize=10)
+
+    # plt.show()
+    # plt.suptitle('loss profile',fontsize=15, y=1)
+    # plt.savefig('loss_comp_sim.jpg')
+
+    return loss, P_scat, P_spec
+
+
+if __name__ == '__main__':
+
+    # dummy_points = np.array([[0,0,0,0,0.1], #
+    #                         [0,0,0,0,0.03],
+    #                         [0,0,0,0,0.006]])
+    
+
+    dummy_points = np.vstack((np.tile([0,0,0,0,0.1],(180,1)),np.tile([0,0,0,0,0.03],(180,1)),np.tile([0,0,0,0,0.001],(180,1))))
+    latest_dummy_points = np.vstack((np.tile([0,0,0,0,4],(180,1)),np.tile([0,0,0,0,2],(180,1)),np.tile([0,0,0,0,1],(180,1))))
+    
+    dummy_rho = np.array([20]*540)
+
+    dummy_angles = np.hstack((np.linspace(-np.pi/2,np.pi/2,180),np.linspace(-np.pi/2,np.pi/2,180),np.linspace(-np.pi/2,np.pi/2,180)))
+
+    radar = radar()
+
+    #loss_old, scat_old, spec_old = get_loss(dummy_points,dummy_rho,dummy_angles,radar)
+    loss_new, scat_new, spec_new = get_loss_2(dummy_points,dummy_rho,dummy_angles,radar)
+    loss_latest, scat_latest, spec_latest = get_loss_3(latest_dummy_points,dummy_rho,dummy_angles,radar)
+    
+
+    # print(f"Old Loss: {loss_old}")
+    # print(f"New Loss: {loss_new}")
+
+    fig, ax = plt.subplots(2,3,  figsize=(10,5))
+
+    #ax[0,0].plot(loss_old)
+    #ax[0,1].plot(spec_old)
+    #ax[0,2].plot(scat_old)
+    ax[0,0].plot(loss_latest)
+    ax[0,0].set_title("New total loss",fontsize=10)
+    ax[0,1].plot(spec_latest)
+    ax[0,1].set_title("New specular loss",fontsize=10)
+    ax[0,2].plot(scat_latest)
+    ax[0,2].set_title("New scatter loss",fontsize=10)
+    ax[1,0].plot(loss_new)
+    ax[1,0].set_title("Prev total loss",fontsize=10)
+    ax[1,1].plot(spec_new)
+    ax[1,1].set_title("Prev specular loss",fontsize=10)
+    ax[1,2].plot(scat_new)
+    ax[1,2].set_title("Prev scatter loss",fontsize=10)
+
+    plt.show()
+    plt.suptitle('0:179 => Metal 180:359 =>Concrete 360:539=>Wood',fontsize=15, y=1)
+    plt.savefig('loss_comparison.jpg')
+

scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/shenron/heatmap_gen.py

@@ -0,0 +1,177 @@
+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
+from noise_utils import get_noise
+import pdb
+
+# @jit(nopython=True)
+def heatmap_gen(rho, theta, loss, speed, radar, plot_fig, return_power):
+
+    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)
+    
+    # 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 = get_noise(radar)
+
+    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.chirpT * speed / radar.c
+            _lambda = radar.c / radar.f
+            sRx = _lambda / 2
+            _lambda = radar.c / radar.f
+            tau_vec = np.zeros((radar.nRx, len(rho)))
+            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 return_power:
+                    if rho[j] != 0:
+                        signal = 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
+            adc_sampled = np.sqrt(radar.gain * _lambda ** 2 / (4 * np.pi) ** 3) * np.conj(sum_samp) * (x)
+            adc_sampled = adc_sampled + signal_Noisy
+            measurement[chirp, :, :] = adc_sampled
+
+        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
+
+        adc_sampled = np.sqrt(radar.gain * _lambda ** 2 / (4 * np.pi) ** 3) * np.conj(sum_samp) * (x)
+        adc_sampled = adc_sampled + signal_Noisy    
+    # 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()
+
+    test_WI=1
+    if (test_WI):
+        points = np.load("E:/Radar_sim/simlator/git repo/Heatmap-sim/simulation5.npy")
+        rho = np.linalg.norm(points[:, 0:3] - np.array(radar.center+radar.elv), axis=1)
+        theta = math.pi / 2 - np.arctan(((points[:, 0] - radar.center[0]) / (points[:, 1] - radar.center[1])))
+        loss = points[:, 3]
+        speed = np.zeros_like(rho)
+    else:
+        rho = np.array([5, 5, 10])  # np.linspace(7.9749, 9.2793, 97)
+        theta = np.array([math.pi / 3, math.pi * 2 / 3, math.pi / 2])  # np.linspace(1.6856, 1.521, 97)
+        loss = np.ones_like(rho)
+        speed = np.zeros_like(rho)
+
+    x_axis, y_axis, plot_data, doppler_data = heatmap_gen(rho, theta, loss, speed, radar, 1)
+
+    diction = {"x_axis": x_axis, "y_axis": y_axis, "plot_data": plot_data}
+    sio.savemat('test_heatmap.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/e2e_agent_sem_lidar2shenron_package/shenron/heatmap_gen_fast.py

@@ -0,0 +1,280 @@
+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():
+    try:
+        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)
+    except Exception:
+        return 0, 0
+
+# @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
+    try:
+        if torch.cuda.is_available():
+            gpu_id, _ = get_gpu_id_most_avlbl_mem()
+            device = torch.device(f'cuda:{gpu_id}')
+        else:
+            gpu_id = 0
+            device = torch.device('cpu')
+    except Exception:
+        gpu_id = 0
+        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)
+    
+
+    ##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))])            
+            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
+            loss_factor = (torch.sqrt(loss)[:,None]) ## from network
+
+            signal_single_antenna = loss_factor*dechirped # (Nx256)
+
+            signal = beamforming_vector*signal_single_antenna[None,:,:] # (80x256)
+
+            signal = torch.squeeze(torch.sum(signal,1))
+
+            # 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 beamforming_vector
+        del signal
+        del signal_single_antenna
+        if device.type == 'cuda':
+            with torch.cuda.device(f'cuda:{gpu_id}'):
+                torch.cuda.empty_cache()
+        # 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/e2e_agent_sem_lidar2shenron_package/simulator_configs.yaml

@@ -0,0 +1,46 @@
+INPUT : "lidar"
+
+NOISE : True
+
+BUILDINGS : True
+
+# RADAR_TYPE : ti_cascade # 1. radarbook | 2. ti_cascade
+RADAR_TYPE : ti_cascade
+
+INDOOR : False
+
+RADAR_MOVING : False
+
+RAY_TRACING : False
+
+INVERT_ANGLE : 0
+
+BASE_PATH : "/radar-imaging-dataset/carla_garage_data/ll_dataset_2023_05_10/Routes_Town04_ll_Repetition0/Town04_ll_route0_11_17_05_03_55/"
+
+CAMERA_PATH : "rgb_image/" #path for original camera images
+
+LIDAR_PATH : "lidar_pcd/" #path for unprocessed lidar point clouds
+
+LIDAR_PATH_SEGMENTED : "lidar_segmented/" #path for segmented lidar point clouds
+
+LIDAR_PATH_CLUSTERS : "lidar_clusters_centroid_with_labels" #path for saving lidar pcd with cluster labels with a separate channel
+ 
+LIDAR_PATH_LABELS : "lidar_with_labels/" #path for saving lidar pcd with cluster labels with a separate channel
+
+LIDAR_PATH_POINT_VELOCITY : "lidar_velocity/" #path for saving the velocity of all the points and all the frames
+
+LIDAR_PATH_CLUSTER_VELOCITY : "lidar_clusters_centroid_with_label_velocity/" #path for saving the velocity of all the cluster points and all the frames
+
+RADAR_PATH_SIMULATED : "radar_data_simulated/"
+
+# CARLA_SHENRON_SEM_LIDAR : carla_shenron_sem_lidar/
+CARLA_SHENRON_SEM_LIDAR : semantic_lidar/
+
+
+# PATHS :
+
+#   LIDAR_PATH : "C:\\Users\\Strai\\Downloads\\semantic_lidar"
+
+#   LIDAR_FOLDERS : ["semantic_lidar"]
+
+#   OUT_PATH : "C:\\Users\\Strai\\Downloads\\semantic_lidar\\semantic_lidar"

scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/transform_dsp_utils.py

@@ -0,0 +1,264 @@
+import numpy as np
+import matplotlib.pyplot as plt
+import torch
+import pdb
+from scipy import signal
+from scipy import io as sio
+import matplotlib.pyplot as plt
+
+def separate_real_imag(input):
+
+    real = input.real
+    imag = input.imag
+
+    out_tensor = np.stack((real,imag), axis=0)
+
+    return out_tensor
+
+def separate_mag_phase(input):
+
+    mag = np.abs(input)
+    phase = np.angle(input)
+
+    out_tensor = np.stack((mag,phase), axis=0)
+
+    return out_tensor
+
+def convert_adc_to_3d_fft(adcData, rangefft_size = 256, dopplerfft_size=128, anglefft_size = 256, window=False, distance_null = False):
+    """
+    Function to convert adc data to RA tensor
+    Input: 
+        adcData: 
+            NxDxM -- where N is the range dimension and M is the angle dimension and D is doppler.
+
+    Output:
+        RA tensor: R-size x A-size
+    """
+    if window:
+        range_hanning = np.hanning(rangefft_size)[:,None,None]
+        range_hanning = np.tile(range_hanning, (1,adcData.shape[1],adcData.shape[2]))
+
+        adcData = adcData*range_hanning
+    
+    if distance_null:
+        radar_range_res = 3e8/(2*2.56e8)
+        range_vector = np.arange(rangefft_size)*radar_range_res
+        range_vector_loss = range_vector**4
+        range_vector_tiled = np.tile(range_vector_loss[:,None, None], (1,adcData.shape[1],adcData.shape[2]))
+        
+        adcData = adcData*range_vector_tiled
+    rangefft = np.fft.fft(adcData, rangefft_size, 0)
+    dopplerfft = np.fft.fft(rangefft, dopplerfft_size, 1)
+    anglefft = np.fft.fftshift(np.fft.fft(dopplerfft, anglefft_size, 2),2)
+
+    # self.anglefft = anglefft
+    return anglefft
+
+def cart2polar(x, y, in_pixels, limit, range_res):
+    
+    #Assuming uniform theta resolution
+    r = np.sqrt((limit-x)**2 + (y)**2)*(1/range_res)
+    
+    theta = np.arctan2((y),(limit-x))
+    theta_px = np.sin(theta)*(in_pixels/2) + in_pixels/2 
+    return r, theta_px
+
+def polar_to_cart(RATensor, range_res = 150/256, in_pixels = 256, limit = 150, out_pixels = 256):
+
+    """
+    convert a polar range angle tensor to cartesian array
+    Input: 
+        RATensor: NxM
+    Output:
+        cart_RATensor: NxM
+    """  
+
+    X, Y = np.meshgrid(np.linspace(0, limit, out_pixels), 
+                            np.linspace(-limit/2, limit/2, out_pixels))
+
+    R_samp, Theta_samp = cart2polar(X, Y, in_pixels, limit, range_res)
+
+    R_samp = R_samp.astype(int)
+    Theta_samp = Theta_samp.astype(int)
+    
+    R_samp[(R_samp>(in_pixels-1))] = 0
+    R_samp[(R_samp<0)] = 0
+    Theta_samp[(Theta_samp>(in_pixels-1))] = 0
+    Theta_samp[(Theta_samp<0)] = 0
+    
+    
+    if RATensor.ndim >2:
+        polar_img = RATensor[...,R_samp,Theta_samp]
+        # polar_img = torch.reshape(polar_img,(RATensor.shape[0],RATensor.shape[1],out_pixels,out_pixels))
+        if torch.is_tensor(RATensor):
+            polar_img = torch.transpose(polar_img,-1,-2)
+        else:
+            polar_img = np.swapaxes(polar_img,-2,-1)
+    else:
+        polar_img = RATensor[R_samp,Theta_samp]
+        polar_img = torch.reshape(polar_img,(out_pixels,out_pixels))
+        polar_img = polar_img.T
+
+    return polar_img
+
+
+
+def plot_data(data, ax = None):
+
+    # if not ax:
+    #     fig, ax = plt.subplots(1, len(data))
+
+    for num, im in enumerate(data):
+        ax[num].imshow(im)
+    
+    # plt.show()
+
+    return ax
+    # plt.imshow(polar_image[0,0])
+    # plt.figure()
+    # plt.imshow(cart_image[0,0])
+
+def CFAR_filtered_output():
+    ##Cfar layers
+    guard_cell = 5
+    CFAR_cell = 11
+    guard_avg = nn.AvgPool2d(guard_cell, stride = 1, padding = 2, divisor_override = 1, count_include_pad=False)
+    CFAR_avg = nn.AvgPool2d(CFAR_cell, stride = 1, padding = 5, divisor_override=1, count_include_pad=False)
+
+    ##Input
+    data = sio.loadmat("lid2rad10.mat")
+    a = data["plot_data"]
+    x_axis = data["x_axis"]
+    y_axis = data["y_axis"]
+
+    (size1,size2) = x_axis.shape
+
+    ##CFAR processing
+    a = torch.tensor(a,dtype=torch.float)
+
+    b = guard_avg(a[None,:,:])
+    c = CFAR_avg(a[None,:,:])
+
+    CA_region = (c-b)/(CFAR_cell**2-guard_cell**2)
+    thrshold = 10
+
+    cfar_indices = np.squeeze(a)>np.squeeze(CA_region)*thrshold
+    cfar_indices[[0,1,2,size2-3,size2-2,size2-1],:] = 0
+    cfar_indices[:,[0,1,2,size2-3,size2-2,size2-1]] = 0
+
+    imag = np.zeros((size1,size2))
+    imag[cfar_indices] = 1
+
+    x_points = x_axis[cfar_indices]
+    y_points = y_axis[cfar_indices]
+    z_points = np.zeros_like(y_points)
+
+    pc = np.vstack((x_points,y_points,z_points))
+    pc = pc.T
+
+    ##Plotting
+    pcd=open3d.open3d.geometry.PointCloud()
+    pcd.points= open3d.open3d.utility.Vector3dVector(pc)
+    open3d.open3d.visualization.draw_geometries([pcd])
+
+    fig, ax = plt.subplots(1,5)
+    ax[0].imshow(a.numpy())
+    ax[1].imshow(np.squeeze(b).numpy())
+    ax[2].imshow(np.squeeze(c).numpy())
+    ax[3].imshow(np.squeeze(CA_region).numpy())
+    ax[4].imshow(imag)
+    plt.show()
+    
+class CA_CFAR():
+    """
+    Description:
+    ------------
+        Cell Averaging - Constant False Alarm Rate algorithm
+        Performs an automatic detection on the input range-Doppler matrix with an adaptive thresholding.
+        The threshold level is determined for each cell in the range-Doppler map with the estimation
+        of the power level of its surrounding noise. The average power of the noise is estimated on a
+        rectangular window, that is defined around the CUT (Cell Under Test). In order the mitigate the effect
+        of the target reflection energy spreading some cells are left out from the calculation in the immediate
+        vicinity of the CUT. These cells are the guard cells.
+        The size of the estimation window and guard window can be set with the win_param parameter.
+    Implementation notes:
+    ---------------------
+        Implementation based on https://github.com/petotamas/APRiL
+    Parameters:
+    -----------
+    :param win_param: Parameters of the noise power estimation window
+                      [Est. window length, Est. window width, Guard window length, Guard window width]
+    :param threshold: Threshold level above the estimated average noise power
+    :type win_param: python list with 4 elements
+    :type threshold: float
+    Return values:
+    --------------
+    """
+
+    def __init__(self, win_param, threshold, rd_size):
+        win_width = win_param[0]
+        win_height = win_param[1]
+        guard_width = win_param[2]
+        guard_height = win_param[3]
+
+        # Create window mask with guard cells
+        self.mask = np.ones((2 * win_height + 1, 2 * win_width + 1), dtype=bool)
+        self.mask[win_height - guard_height:win_height + 1 + guard_height, win_width - guard_width:win_width + 1 + guard_width] = 0
+
+        # Convert threshold value
+        self.threshold = 10 ** (threshold / 10)
+
+        # Number cells within window around CUT; used for averaging operation.
+        self.num_valid_cells_in_window = signal.convolve2d(np.ones(rd_size, dtype=float), self.mask, mode='same')
+
+    def __call__(self, rd_matrix):
+        """
+        Description:
+        ------------
+            Performs the automatic detection on the input range-Doppler matrix.
+        Implementation notes:
+        ---------------------
+        Parameters:
+        -----------
+        :param rd_matrix: Range-Doppler map on which the automatic detection should be performed
+        :type rd_matrix: R x D complex numpy array
+        Return values:
+        --------------
+        :return hit_matrix: Calculated hit matrix
+        """
+        # Convert range-Doppler map values to power
+        rd_matrix = np.abs(rd_matrix) ** 2
+
+        # Perform detection
+        rd_windowed_sum = signal.convolve2d(rd_matrix, self.mask, mode='same')
+        rd_avg_noise_power = rd_windowed_sum / self.num_valid_cells_in_window
+        rd_snr = rd_matrix / rd_avg_noise_power
+        hit_matrix = rd_snr > self.threshold
+
+        return hit_matrix
+
+
+if __name__ == "__main__":
+
+    sim_adc = sio.loadmat("/home/Kshitiz/semantic_lidar/radar/0099.mat")
+    sim_adc_data = sim_adc['adc_data']
+    sim_adc_data = np.transpose(sim_adc_data, (2,0,1))
+    
+    rangefft_size = 256
+    anglefft_size = 256
+    dopplerfft_size = 128
+    
+    sim_RDA = convert_adc_to_3d_fft(sim_adc_data ,rangefft_size,dopplerfft_size, anglefft_size, distance_null=False)
+
+    sim_dra = np.transpose(sim_RDA,(1,0,2))
+    
+    limit = 50
+    
+    sim_dra_cart = abs(polar_to_cart(sim_dra,limit=limit,out_pixels=256))
+
+    fig ,ax = plt.subplots(1,2)
+
+    ax[0].imshow(abs(sim_dra[0,:,:]))
+    ax[1].imshow(abs(sim_dra_cart[0,:,:]))
+
+    plt.show()

scripts/ISOLATE/model_wrapper.py

@@ -0,0 +1,118 @@
+import sys
+import os
+import numpy as np
+
+# Add the necessary directories to sys.path to ensure internal imports work
+current_dir = os.path.dirname(os.path.abspath(__file__))
+package_root = os.path.join(current_dir, 'e2e_agent_sem_lidar2shenron_package')
+utils_root = os.path.join(current_dir, 'sim_radar_utils')
+
+if current_dir not in sys.path:
+    sys.path.append(current_dir)
+if package_root not in sys.path:
+    sys.path.append(package_root)
+if utils_root not in sys.path:
+    sys.path.append(utils_root)
+
+# Now import the modules
+from e2e_agent_sem_lidar2shenron_package.lidar import run_lidar
+from e2e_agent_sem_lidar2shenron_package.ConfigureRadar import radar
+from sim_radar_utils.radar_processor import RadarProcessor
+from sim_radar_utils.utils_radar import reformat_adc_shenron
+
+class ShenronRadarModel:
+    def __init__(self, radar_type='radarbook'):
+        """
+        Initialize the Shenron Radar Model.
+        
+        Args:
+            radar_type (str): Type of radar to simulate (default: 'radarbook').
+        """
+        print(f"Initializing ShenronRadarModel with type: {radar_type}")
+        self.radar_type = radar_type
+        
+        # Initialize the hardware radar object
+        self.radar_obj = radar(radar_type)
+        self.radar_obj.center = np.array([0.0, 0.0])  # center of radar
+        self.radar_obj.elv = np.array([0.0])
+        
+        # Synchronize global config used by Signal Processor with the Simulated Hardware
+        self._sync_configs()
+
+        # Initialize the signal processor (FFT, CFAR, etc.)
+        self.processor = RadarProcessor()
+        
+        # Standard simulation config used by the internal physics engine
+        self.sim_config = {
+            'RADAR_TYPE': radar_type,
+            'INVERT_ANGLE': 0,
+            'RAY_TRACING': False,
+            'RADAR_MOVING': False
+        }
+
+    def _sync_configs(self):
+        """Important: Sync global variables in sim_radar_utils to match current radar.obj"""
+        import sim_radar_utils.utils_radar as ur
+        
+        # Update Radar Cfg
+        ur.radarCfg['N'] = self.radar_obj.N_sample
+        ur.radarCfg['Np'] = self.radar_obj.chirps
+        ur.radarCfg['NrChn'] = self.radar_obj.nRx
+        ur.radarCfg['fStrt'] = self.radar_obj.f
+        ur.radarCfg['fStop'] = self.radar_obj.f + self.radar_obj.B
+        ur.radarCfg['Tp'] = self.radar_obj.chirp_rep
+        
+        # Update FFT Cfg
+        ur.fftCfg['NFFT'] = self.radar_obj.N_sample
+        ur.fftCfg['NFFTVel'] = self.radar_obj.chirps
+        
+        print(f"Synced global config: N={ur.radarCfg['N']}, Np={ur.radarCfg['Np']}, Ant={ur.radarCfg['NrChn']}")
+
+    def process(self, semantic_lidar_data):
+        """
+        Process semantic LiDAR data to generate a rich radar point cloud.
+        
+        Args:
+            semantic_lidar_data (np.ndarray): Array of shape [N, 7] 
+                format: [x, y, z, intensity, cos_inc_angle, object_idx, semantic_tag]
+                
+        Returns:
+            np.ndarray: Rich radar point cloud [M, 5]
+                format: [x, y, z, velocity, magnitude]
+        """
+        if semantic_lidar_data is None or len(semantic_lidar_data) == 0:
+            return np.empty((0, 5))
+            
+        try:
+            # 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)
+            
+            # 2. Reformat to match Signal Processor expectations
+            # Internal logic often needs specific axis ordering
+            adc_data = reformat_adc_shenron(adc_data)
+            
+            # 3. Fast Fourier Transform (FFT) Pipeline
+            # Range FFT converts time data to range profiles
+            range_profile = self.processor.cal_range_fft(adc_data)
+            
+            # Doppler FFT converts range profiles over time to velocity info
+            doppler_profile = self.processor.cal_doppler_fft(range_profile)
+            
+            # 4. Target Detection and Rich Parameter Extraction
+            # CFAR detection + Angle of Arrival (AoA) estimation
+            # returns: rangeAoA, pointcloud ([x, y, z, vel, mag])
+            _, rich_pcd = self.processor.convert_to_pcd(doppler_profile)
+            
+            return rich_pcd
+            
+        except Exception as e:
+            print(f"Error during Shenron processing: {e}")
+            import traceback
+            traceback.print_exc()
+            return np.empty((0, 5))
+
+if __name__ == "__main__":
+    # Internal test/demo
+    model = ShenronRadarModel()
+    print("Model initialized successfully.")

scripts/ISOLATE/sim_radar_utils/cfar_detector.py

@@ -0,0 +1,70 @@
+import numpy as np
+from scipy import signal
+
+class CA_CFAR():
+    """
+    Description:
+    ------------
+        Cell Averaging - Constant False Alarm Rate algorithm
+        Performs an automatic detection on the input range-Doppler matrix with an adaptive thresholding.
+        The threshold level is determined for each cell in the range-Doppler map with the estimation
+        of the power level of its surrounding noise. The average power of the noise is estimated on a
+        rectangular window, that is defined around the CUT (Cell Under Test). In order the mitigate the effect
+        of the target reflection energy spreading some cells are left out from the calculation in the immediate
+        vicinity of the CUT. These cells are the guard cells.
+        The size of the estimation window and guard window can be set with the win_param parameter.
+    Implementation notes:
+    ---------------------
+        Implementation based on https://github.com/petotamas/APRiL
+    Parameters:
+    -----------
+    :param win_param: Parameters of the noise power estimation window
+                      [Est. window length, Est. window width, Guard window length, Guard window width]
+    :param threshold: Threshold level above the estimated average noise power
+    :type win_param: python list with 4 elements
+    :type threshold: float
+    Return values:
+    --------------
+    """
+
+    def __init__(self, win_param, threshold, rd_size):
+        win_width = win_param[0]
+        win_height = win_param[1]
+        guard_width = win_param[2]
+        guard_height = win_param[3]
+
+        # Create window mask with guard cells
+        self.mask = np.ones((2 * win_height + 1, 2 * win_width + 1), dtype=bool)
+        self.mask[win_height - guard_height:win_height + 1 + guard_height, win_width - guard_width:win_width + 1 + guard_width] = 0
+
+        # Convert threshold value
+        self.threshold = 10 ** (threshold / 10)
+
+        # Number cells within window around CUT; used for averaging operation.
+        self.num_valid_cells_in_window = signal.convolve2d(np.ones(rd_size, dtype=float), self.mask, mode='same')
+
+    def __call__(self, rd_matrix):
+        """
+        Description:
+        ------------
+            Performs the automatic detection on the input range-Doppler matrix.
+        Implementation notes:
+        ---------------------
+        Parameters:
+        -----------
+        :param rd_matrix: Range-Doppler map on which the automatic detection should be performed
+        :type rd_matrix: R x D complex numpy array
+        Return values:
+        --------------
+        :return hit_matrix: Calculated hit matrix
+        """
+        # Convert range-Doppler map values to power
+        rd_matrix = np.abs(rd_matrix) ** 2
+
+        # Perform detection
+        rd_windowed_sum = signal.convolve2d(rd_matrix, self.mask, mode='same')
+        rd_avg_noise_power = rd_windowed_sum / self.num_valid_cells_in_window
+        rd_snr = rd_matrix / rd_avg_noise_power
+        hit_matrix = rd_snr > self.threshold
+
+        return hit_matrix

scripts/ISOLATE/sim_radar_utils/config.yaml

@@ -0,0 +1,101 @@
+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
+  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
+
+
+FFT:
+  NFFT: 256 #512
+  NFFTVel: 128
+  NFFTAnt: 256 #128
+  RMin: 0
+  RMax: 1000
+  c0: 299792458
+
+
+Cluster:
+  xlim: [-20, 20]
+  ylim: [0, 40]
+  zlim: [0, 2]
+
+
+ROS:
+  Topic:
+    radarCfg: '/radarbook/config'
+    radarRaw: '/radarbook/data'
+    radarPCD: '/radarbook/pcd'
+    cameraRGB: '/camera/color/image_raw'
+    cameraDepth: '/camera/depth/image_rect_raw'
+    lidarPCD: '/os1_cloud_node/points'
+  SavePath:
+    radarCfg: '/radar_cfg.json'
+    radarRaw: '/radar_data/'
+    radarPCD: '/radar_pcd/'
+    cameraRGB: '/rgb_image/'
+    # cameraRGB: '/camera_original/'
+    cameraDepth: '/depth_image/'
+    # lidarPCD: '/lidar_pcd/'
+    lidarPCD: '/raw_lidar/'
+    timeStamp: '/time_stamp.npy'
+
+CFAR:
+  win_param: [9, 9, 3, 3]
+  threshold: 20
+
+
+Visualize:
+  rangeAoA:
+    title: "Range-Angle Plot"
+    xLabel: "Range"
+    xUnit: "m"
+    yLabel: "Angle"
+    yUnit: "Degree"
+    winSize: [500, 400]
+    pos: [50, 50]
+  rangeDoppler:
+    title: "Range-Velocity Plot"
+    xLabel: "Range"
+    xUnit: "m"
+    yLabel: "Velocity"
+    yUnit: "m/s"
+    winSize: [500, 400]
+    pos: [600, 50]
+  radarPCD:
+    title: "Radar Point Cloud"
+    xLabel: "Azimuth Position"
+    xUnit: "m"
+    yLabel: "Depth Position"
+    yUnit: "m"
+    winSize: [500, 400]
+    pos: [1200, 50]
+    penWidth: 5
+    penColor: 'r'
+    symbol: 'o'
+    dotSize: 1
+  cameraRGB:
+    title: "RGB Image"
+    winSize: [640, 360]
+    pos: [50, 500]
+  cameraDepth:
+    title: "Depth Image"
+    winSize: [640, 360]
+    pos: [600, 500]
+  lidarPCD:
+    title: "Lidar Point Cloud"
+    xRange: [-30, 30]
+    yRange: [-30, 30]
+    winSize: [500, 400]
+    pos: [1200, 500]
+    dotSize: 1
+

scripts/ISOLATE/sim_radar_utils/convert2D_img.py

@@ -0,0 +1,116 @@
+import numpy as  np
+import os
+import pickle as pkl
+from PIL import Image
+import json
+import yaml
+# import ffmpeg
+import matplotlib.pyplot as plt
+# import cv2
+import sys
+import glob
+
+# sys.path.append('/radar-imaging-dataset/mmfn_project/mmfn_scripts/team_code/mmfn_utils/sim_radar_utils/')
+# sys.path.append('/radar-imaging-dataset/mmfn_project/mmfn_scripts/team_code/e2e_agent_sem_lidar2shenron_package/')
+
+from e2e_agent_sem_lidar2shenron_package.lidar import run_lidar
+
+from sim_radar_utils.radar_processor import RadarProcessor
+from sim_radar_utils.utils_radar import *
+from sim_radar_utils.transform_utils import *
+
+def convert2D_img_func(sim_radar, limit = 75):
+    '''
+    converts the 3D radar raw data to 2d range-angle image of dimension 256X256
+    '''
+    radarProcessor = RadarProcessor()
+    # radar = sim_radar[0]
+    radar = sim_radar
+    
+    chirpLevelData = reformat_adc_shenron(radar)
+    rangeProfile = radarProcessor.cal_range_fft(chirpLevelData)
+    # dopplerProfile = radarProcessor.cal_doppler_fft(rangeProfile)
+    aoaProfile = radarProcessor.cal_angle_fft(rangeProfile)
+    range_angle = get_range_angle(aoaProfile)
+    cart_cord = polar_to_cart(range_angle, limit = limit)
+    # print(cart_cord_man.shape)
+    # return [cart_cord]
+    return cart_cord
+
+def convert_sem_lidar_2D_img_func(sim_radar, invert_angle, limit = 75):
+    '''
+    converts the semantic_lidar data to the range angle 2D image of dimension 256X256
+    '''
+    
+    # convert semantic lidar to raw 3d radar data
+    
+    # with open('/radar-imaging-dataset/mmfn_project/mmfn_scripts/team_code/e2e_agent_sem_lidar2shenron_package/simulator_configs.yaml', 'r') as f:
+    #     sim_config = yaml.safe_load(f)
+    
+    # with open(glob.glob('../carla_garage_radar/team_code/e2e_agent_sem_lidar2shenron_package/simulator_configs.yaml'), 'r') as f:
+    #     sim_config = yaml.safe_load(f)
+    
+    with open('/radar-imaging-dataset/carla_garage_radar/team_code/e2e_agent_sem_lidar2shenron_package/simulator_configs.yaml', 'r') as f:
+        sim_config = yaml.safe_load(f)
+    
+    sim_config['INVERT_ANGLE'] = invert_angle
+    
+    radar = run_lidar(sim_config, sim_radar)
+       
+    radarProcessor = RadarProcessor()
+    # radar = sim_radar[0]
+    # radar = sim_radar
+    
+    chirpLevelData = reformat_adc_shenron(radar)
+    rangeProfile = radarProcessor.cal_range_fft(chirpLevelData)
+    # dopplerProfile = radarProcessor.cal_doppler_fft(rangeProfile)
+    aoaProfile = radarProcessor.cal_angle_fft(rangeProfile)
+    range_angle = get_range_angle(aoaProfile)
+    cart_cord = polar_to_cart(range_angle, limit = limit)
+    # print(cart_cord_man.shape)
+    # return [cart_cord]
+    return cart_cord
+    
+def main():
+    radarProcessor = RadarProcessor()
+
+    # with open('/mnt/intA-ssdr1-4tb/satyam53/mmfn/data1/pro_train_fnss_0702/0.pkl', 'rb') as f:
+    #     pkl_data = pkl.load(f)
+    
+    # print(type(pkl_data))
+    # print(pkl_data.keys())
+    
+    # sim_radar = pkl_data['sim_radar']
+    # # sim_radar = pkl_data['vectormaps']   
+    
+    # print(type(sim_radar[0]))
+    # print(len(sim_radar))
+    # print(sim_radar[0])
+    # radar_man = sim_radar[0]
+    
+    
+    # # radar_man = np.load(f'{path_sim_man}/{files_sim_manual[i]}')
+    # chirpLevelData = reformat_adc_shenron(radar_man)
+    # rangeProfile = radarProcessor.cal_range_fft(chirpLevelData)
+    # # dopplerProfile = radarProcessor.cal_doppler_fft(rangeProfile)
+    # aoaProfile = radarProcessor.cal_angle_fft(rangeProfile)
+    # range_angle = get_range_angle(aoaProfile)
+    # cart_cord_man = polar_to_cart(range_angle, limit = 50)
+    # print(cart_cord_man.shape)
+    
+    # fig, (ax1, ax2, ax3) = plt.subplots(1, 3, figsize=(15,5))
+    # # ax1.imshow(cart_cord, aspect='auto')
+    # # ax1.set_title('radar data simulated using shenron')
+
+    # ax2.imshow(cart_cord_man, aspect='auto')
+    # ax2.set_title('radar data simulated manually')
+    # # plt.show()
+    # # ax3.scatter(lidar[:, 0], lidar[:, 1], s = 0.1, c = lidar[:, 5])
+    # # ax3.set_title('lidar data')
+    # plt.tight_layout()
+    # plt.show()
+    # # plt.savefig(f'{save_path}/{i}.png')
+    # plt.close()
+    
+if __name__=="__main__":
+    main()

scripts/ISOLATE/sim_radar_utils/radar_processor.py

@@ -0,0 +1,96 @@
+import numpy as np
+import yaml
+import sys
+import glob
+
+# sys.path.append(glob.glob('../carla_garage_radar/team_code/sim_radar_utils/'))
+sys.path.append('radar-imaging-dataset/carla_garage_radar/team_code/sim_radar_utils/')
+# sys.path.append('radar-imaging-dataset/carla_garage_radar/team_code/')
+
+
+# sys.path.append("/radar-imaging-dataset/mmfn_project/mmfn_scripts/team_code/sim_radar_utils/")
+
+from scipy import signal
+from scipy.fft import fft, fftshift
+from sim_radar_utils.cfar_detector import CA_CFAR
+from sim_radar_utils.utils_radar import *
+
+
+cfarCfg = config['CFAR']
+
+
+class RadarProcessor:
+    def __init__(self):
+        # radar data will be shaped as (# of chirp, # of sample, # of antenna)
+        # self.rangeWin = np.tile(signal.windows.hann(radarCfg['N']), (radarCfg['Np'], len(radarCfg['AntIdx']), 1))
+        self.rangeWin = np.tile(signal.windows.hann(radarCfg['N']), (radarCfg['Np'], radarCfg['NrChn'], 1))
+        self.rangeWin = np.transpose(self.rangeWin, (2, 0, 1))
+
+        # self.velWin = np.tile(signal.windows.hann(radarCfg['Np']), (radarCfg['N'], len(radarCfg['AntIdx']), 1))
+        self.velWin = np.tile(signal.windows.hann(radarCfg['Np']), (radarCfg['N'], radarCfg['NrChn'], 1))
+        self.velWin = np.transpose(self.velWin, (0, 2, 1))
+
+        rangeRes = fftCfg['c0'] / (2*(radarCfg['fStop'] - radarCfg['fStrt']))
+        self.rangeAxis = np.arange(0, fftCfg['NFFT'])*radarCfg['N']/fftCfg['NFFT']*rangeRes
+        self.RMinIdx = np.argmin(np.abs(self.rangeAxis - fftCfg['RMin']))
+        self.RMaxIdx = np.argmin(np.abs(self.rangeAxis - fftCfg['RMax']))
+        self.rangeAxis = self.rangeAxis[self.RMinIdx:self.RMaxIdx+1]
+
+        self.angleAxis = np.arcsin(2 * np.arange(-fftCfg['NFFTAnt']/2, fftCfg['NFFTAnt']/2) / fftCfg['NFFTAnt'])
+
+        fc = (radarCfg['fStop'] + radarCfg['fStrt'])/2
+        self.velAxis = np.arange(-fftCfg['NFFTVel']//2, fftCfg['NFFTVel']//2)/fftCfg['NFFTVel']*(1/radarCfg['Tp'])*fftCfg['c0']/(2*fc)
+
+        self.cfar = CA_CFAR(win_param=cfarCfg['win_param'], 
+                            threshold=cfarCfg['threshold'], 
+                            rd_size=(self.RMaxIdx - self.RMinIdx + 1, fftCfg['NFFTVel']))
+
+    def cal_range_fft(self, data):
+        '''apply range window and doppler window and apply fft on each sample to get range profile'''
+        return fft(data * self.rangeWin * self.velWin, fftCfg['NFFT'], 0)
+
+    def cal_doppler_fft(self, rangeProfile):
+        '''apply fft on each chirp to get doppler profile'''
+        return fftshift(fft(rangeProfile[self.RMinIdx:self.RMaxIdx+1, :], fftCfg['NFFTVel'], 1), 1)
+
+    def cal_angle_fft(self, rangeProfile):
+        '''# apply fft on each antenna to get angle profile'''
+        return fftshift(fft(rangeProfile[self.RMinIdx:self.RMaxIdx+1, :], fftCfg['NFFTAnt'], 2), 2)
+
+    def convert_to_pcd(self, dopplerProfile):
+        avgDopplerProfile = np.squeeze(np.mean(dopplerProfile, 2))
+
+        # detect useful peaks using CFAR
+        detections = self.cfar(np.square(np.abs(avgDopplerProfile)))
+
+        # identify range bin and velocity bin for each detected point
+        rowSel, colSel = np.nonzero(detections)
+
+        # pointSel = np.zeros(shape=(len(rowSel), len(radarCfg['AntIdx'])), dtype=complex)
+        pointSel = np.zeros(shape=(len(rowSel), radarCfg['NrChn']), dtype=complex)
+        for i, (row, col) in enumerate(zip(rowSel, colSel)):
+            pointSel[i] = dopplerProfile[row, col, :]
+
+        # calculate range and anlge value
+        rangeVals = self.rangeAxis[rowSel]
+        aoaProfile = fftshift(fft(pointSel, fftCfg['NFFTAnt'], 1), 1)
+        angleIdx = np.argmax(np.abs(aoaProfile), axis=1)
+        angleVals = self.angleAxis[angleIdx]
+
+        # Extract velocity from velocity axis
+        velVals = self.velAxis[colSel]
+
+        # Extract peak magnitude
+        magVals = np.abs(avgDopplerProfile[rowSel, colSel])
+
+        rangeAoA = np.transpose(np.stack([rangeVals, angleVals]))
+
+        # convert Range-AoA to pointcloud
+        # Format: [x, y, z, velocity, magnitude]
+        x = rangeVals * np.cos(angleVals)
+        y = rangeVals * np.sin(angleVals)
+        z = np.zeros_like(x) # 2D radar assumption, z=0
+
+        pointcloud = np.stack([x, y, z, velVals, magVals], axis=1)
+
+        return rangeAoA, pointcloud

scripts/ISOLATE/sim_radar_utils/transform_utils.py

@@ -0,0 +1,91 @@
+import numpy as np
+import torch
+
+def get_range_angle(raw_radar_data):
+    range_angle = np.abs(raw_radar_data).sum(axis=1)
+    return range_angle
+
+def cart2polar(x, y, in_pixels, range_res):
+    # Dividing by range_res to get the range in pixels
+    r = np.sqrt(x ** 2 + y ** 2) * (1 / range_res)
+    
+    #Assuming uniform theta resolution
+    theta = np.arctan2(y, x)
+    theta_px = np.sin(theta) * (in_pixels / 2) + in_pixels / 2 
+    return r, theta_px
+
+def polar_to_cart(RATensor, range_res = 150 / 256, in_pixels = 256, limit = 150, out_pixels = 256):
+
+    """
+    convert a polar range angle tensor to cartesian array
+    Input: 
+        RATensor: NxM
+    Output:
+        cart_RATensor: NxM
+    """  
+
+    X, Y = np.meshgrid(np.linspace(-limit/2, limit/2, out_pixels), np.linspace(-limit/2, limit/2, out_pixels))
+    # Rotating the axes to make the plane point upwards
+    X = np.rot90(X)
+    Y = np.rot90(Y)
+    
+    R_samp, Theta_samp = cart2polar(X, Y, in_pixels, range_res)
+
+    R_samp = R_samp.astype(int)
+    Theta_samp = Theta_samp.astype(int)
+    
+    R_samp[(R_samp > (in_pixels - 1))] = in_pixels - 1
+    R_samp[(R_samp < 0)] = 0
+    Theta_samp[(Theta_samp > (in_pixels - 1))] = in_pixels - 1
+    Theta_samp[(Theta_samp < 0)] = 0
+    
+    if RATensor.ndim >2:
+        polar_img = RATensor[...,R_samp,Theta_samp]
+    else:
+        polar_img = RATensor[R_samp, Theta_samp]
+
+    # Applying mask to extract the front-side view only
+    if out_pixels % 2 == 0:
+        slice_idx = int(out_pixels / 2)
+    else:
+        slice_idx = int(out_pixels / 2) + 1
+    
+    polar_img[slice_idx:, :] = 0
+    
+    return polar_img
+
+# def polar_to_cart(RATensor, range_res = 150/256, in_pixels = 256, limit = 150, out_pixels = 256):
+
+#     """
+#     convert a polar range angle tensor to cartesian array
+#     Input: 
+#         RATensor: NxM
+#     Output:
+#         cart_RATensor: NxM
+#     """  
+
+#     X, Y = np.meshgrid(np.linspace(0, limit, out_pixels), np.linspace(-limit/2, limit/2, out_pixels))
+
+#     R_samp, Theta_samp = cart2polar(X, Y, in_pixels, range_res)
+
+#     R_samp = R_samp.astype(int)
+#     Theta_samp = Theta_samp.astype(int)
+    
+#     R_samp[(R_samp>(in_pixels-1))] = 0
+#     R_samp[(R_samp<0)] = 0
+#     Theta_samp[(Theta_samp>(in_pixels-1))] = 0
+#     Theta_samp[(Theta_samp<0)] = 0
+    
+#     if RATensor.ndim >2:
+#         polar_img = RATensor[...,R_samp,Theta_samp]
+#         # polar_img = torch.reshape(polar_img,(RATensor.shape[0],RATensor.shape[1],out_pixels,out_pixels))
+#         if torch.is_tensor(RATensor):
+#             polar_img = torch.transpose(polar_img,-1,-2)
+#         else:
+#             polar_img = np.swapaxes(polar_img,-2,-1)
+#     else:
+#         polar_img = RATensor[R_samp,Theta_samp]
+#         # polar_img = torch.reshape(polar_img,(out_pixels,out_pixels))
+#         polar_img = polar_img.T
+
+#     return polar_img

scripts/ISOLATE/sim_radar_utils/utils_radar.py

@@ -0,0 +1,112 @@
+import sys, os
+import yaml
+import numpy as np
+# import open3d
+from pyntcloud import PyntCloud
+from PIL import Image
+import matplotlib.pyplot as plt
+import json
+
+
+with open(os.path.join(os.path.abspath(os.path.dirname(__file__)), 'config.yaml'), 'r') as f:
+	config = yaml.safe_load(f)
+	radarCfg = config['radar']
+	savePath = config['ROS']['SavePath']
+	fftCfg = config['FFT']
+	clstCfg = config['Cluster']
+	radarCfg['fStrt'] = float(radarCfg['fStrt'])
+	radarCfg['fStop'] = float(radarCfg['fStop']) 
+	radarCfg['TRampUp'] = float(radarCfg['TRampUp'])
+	radarCfg['TInt'] = float(radarCfg['TInt'])
+	radarCfg['Tp'] = float(radarCfg['Tp'])
+	radarCfg['IniTim'] = float(radarCfg['IniTim'])
+
+
+def config_save_path(dirc):
+	for node in savePath.keys():
+		savePath[node] = dirc + savePath[node]
+
+
+def create_folder(activeNode):
+	for node in activeNode:
+		if node != 'timeStamp' and node != 'radarCfg':
+			os.makedirs(savePath[node], exist_ok=True)
+
+
+def read_time_stamp():
+	return np.load(savePath['timeStamp'])
+
+
+def load_radar_cfg():
+	with open(savePath['radarCfg'], 'r') as f:
+		newCfg = json.load(f)
+		for key in radarCfg.keys():
+			radarCfg[key] = newCfg[key]
+
+#reformat adc shenron
+def reformat_adc_shenron(data):
+	data = np.swapaxes(data, 1, 2)
+	# print(data.shape)
+	chirpLevelData = data
+	# return np.transpose(chirpLevelData[:, :, radarCfg['AntIdx']], (1, 0, 2))
+	return np.transpose(chirpLevelData[:, :, :], (1, 0, 2))
+
+
+def reformat_adc(data):
+	data = np.reshape(data, (radarCfg['Np'], len(radarCfg['TxSeq']), radarCfg['N'], radarCfg['NrChn']))
+	chirpLevelData = data[:, 0, :, :]
+	for j in range(1, len(radarCfg['TxSeq'])):
+		chirpLevelData = np.concatenate((chirpLevelData, data[:, j, :, :]), axis=-1)
+	return np.transpose(chirpLevelData[:, :, radarCfg['AntIdx']], (1, 0, 2))
+
+
+def read_radar_data(frame):
+	radarData = np.load(savePath['radarRaw'] + f'{frame:04}.npy')
+	chirpLevelData = reformat_adc(radarData)
+	return chirpLevelData
+
+#read radar data shenron
+def read_radar_data_shenron(frame):
+	radarData = np.load(savePath['radarRaw'] + f'{frame:04}.npy')
+	chirpLevelData = reformat_adc_shenron(radarData)
+	return chirpLevelData
+
+
+def read_radar_pcd(frame):
+	points = np.load(savePath['radarPCD'] + f'{frame:04}.npy')
+	return points
+
+
+def read_camera_img(frame):
+	rgbImg = np.asarray(Image.open(savePath['cameraRGB'] + f'{frame:04}.png'))
+	depthImg = np.asarray(Image.open(savePath['cameraDepth'] + f'{frame:04}.png'))
+	return rgbImg, depthImg
+
+
+def read_lidar_pcd(frame):
+	pcd = PyntCloud.from_file(savePath['lidarPCD'] + f'{frame:04}.pts')
+	return pcd.xyz
+
+
+def plot_range_doppler(vis, dopplerProfile, rangeAxis, velAxis):
+	dopplerProfiledB = 20*np.log10(np.abs(dopplerProfile[:, :, 0]))
+	dopplerProfileMax = np.max(dopplerProfiledB)
+	dopplerProfileNorm = dopplerProfiledB - dopplerProfileMax
+	dopplerProfileNorm[dopplerProfileNorm < -30] = -30
+	vis.plot_depth_fig(
+		data=dopplerProfileNorm,
+		pos=[velAxis[0], fftCfg['RMin']], 
+		scale=[(velAxis[-1] - velAxis[0])/fftCfg['NFFTVel'], (fftCfg['RMax'] - fftCfg['RMin'])/len(rangeAxis)]
+	)
+	
+
+def plot_range_aoa(vis, aoaProfile, rangeAxis):
+	aoaProfiledB = 20*np.log10(np.abs(aoaProfile[:, 1, :]))
+	aoaProfileMax = np.max(aoaProfiledB)
+	aoaProfileNorm = aoaProfiledB - aoaProfileMax
+	aoaProfileNorm[aoaProfileNorm < -30] = -30
+	vis.plot_depth_fig(
+		data=aoaProfileNorm, 
+		pos=[-1, fftCfg['RMin']],
+		scale=[2.0/fftCfg['NFFTAnt']/np.pi*180, (fftCfg['RMax'] - fftCfg['RMin'])/len(rangeAxis)]
+	)

scripts/data_to_mcap.py

@@ -97,6 +97,7 @@ def convert_folder(folder_path):
         lidar_channel_id = writer.register_channel(topic="/lidar", message_encoding="json", schema_id=lidar_schema_id)
         pose_channel_id = writer.register_channel(topic="/ego_pose", message_encoding="json", schema_id=pose_schema_id)
         radar_channel_id = writer.register_channel(topic="/radar", message_encoding="json", schema_id=lidar_schema_id)
+        shenron_channel_id = writer.register_channel(topic="/radar/shenron", message_encoding="json", schema_id=lidar_schema_id)
 
         frame_count = 0
         for frame in load_frames(folder_path):
@@ -189,6 +190,34 @@ def convert_folder(folder_path):
                     }
                     writer.add_message(radar_channel_id, log_time=ts_ns, data=json.dumps(radar_msg).encode(), publish_time=ts_ns)
 
+            # SHENRON RADAR
+            shenron_file = f"frame_{self.frame_id:06d}.npy"
+            shenron_path = os.path.join(folder_path, "shenron_radar", shenron_file)
+            if os.path.exists(shenron_path):
+                s_data = np.load(shenron_path)
+                if s_data.size > 0:
+                    # s_data = [x, y, z, velocity, magnitude]
+                    # ISOLATE coords: X is fwd, Y is right.
+                    # ROS: X is fwd, Y is left.
+                    ros_shenron = s_data.copy().astype(np.float32)
+                    ros_shenron[:, 1] = -ros_shenron[:, 1] # Negate Y for ROS
+                    
+                    shenron_msg = {
+                        "timestamp": {"sec": ts_sec, "nsec": ts_nsec},
+                        "frame_id": "ego_vehicle", 
+                        "pose": identity_pose, 
+                        "point_stride": 20, # 5 floats * 4 bytes
+                        "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_channel_id, log_time=ts_ns, data=json.dumps(shenron_msg).encode(), publish_time=ts_ns)
+
             frame_count += 1
             if frame_count % 50 == 0:
                 print(f"  Processed {frame_count} frames...", flush=True)

scripts/generate_shenron.py

@@ -0,0 +1,95 @@
+import os
+import sys
+import numpy as np
+import tqdm
+from pathlib import Path
+
+# 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'))
+
+# Import the model wrapper
+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)
+
+def process_session(session_path):
+    print(f"\n>>> Processing session: {session_path.name}")
+    
+    lidar_dir = session_path / "lidar"
+    if not lidar_dir.exists():
+        print(f"    [SKIP] No 'lidar' folder found.")
+        return
+        
+    # Find all .npy files in lidar/
+    lidar_files = sorted(list(lidar_dir.glob("*.npy")))
+    if not lidar_files:
+        print(f"    [SKIP] No .npy files in 'lidar' folder.")
+        return
+
+    output_dir = session_path / "shenron_radar"
+    output_dir.mkdir(exist_ok=True)
+    
+    # Initialize the model once per session
+    print(f"    Initializing ShenronRadarModel...")
+    model = ShenronRadarModel(radar_type='radarbook')
+    
+    print(f"    Generating Shenron Radar data for {len(lidar_files)} frames...")
+    
+    for lidar_file in tqdm.tqdm(lidar_files, desc="      Simulating Radar", unit="frame"):
+        try:
+            # 1. Load Semantic LiDAR data
+            # Expected raw: [x, y, z, cos, obj, tag] (6 cols)
+            # Expected Shenron input: [x, y, z, intensity, cos, obj, tag] (7 cols)
+            data = np.load(lidar_file)
+            
+            if data.shape[1] == 6:
+                # Pad with a dummy intensity column at index 3
+                # This aligns 'tag' to index 6 as expected by our lidar.py mapping
+                padded_data = np.zeros((data.shape[0], 7))
+                padded_data[:, 0:3] = data[:, 0:3] # x, y, z
+                padded_data[:, 4:7] = data[:, 3:6] # cos, obj, tag
+                data = padded_data
+            
+            # 2. Process through the physics-based model
+            # returns rich PCD: [M, 5] (x, y, z, velocity, magnitude)
+            rich_pcd = model.process(data)
+            
+            # 3. Save to disk
+            output_file = output_dir / lidar_file.name
+            np.save(output_file, rich_pcd)
+            
+        except Exception as e:
+            print(f"\n    [ERROR] Failed to process {lidar_file.name}: {e}")
+
+def main():
+    data_root = project_root / "data"
+    if not data_root.exists():
+        print(f"Error: {data_root} not found.")
+        return
+        
+    # Get all session folders
+    sessions = sorted([d for d in data_root.iterdir() if d.is_dir()])
+    
+    if not sessions:
+        print("No simulation sessions found in data/.")
+        return
+        
+    print(f"Found {len(sessions)} sessions.")
+    
+    for session in sessions:
+        # Check if the session has frames.jsonl to confirm it's a valid data folder
+        if (session / "frames.jsonl").exists():
+            process_session(session)
+        else:
+            print(f"Skipping {session.name} (no frames.jsonl found).")
+            
+    print("\n" + "="*50)
+    print("SHENRON BATCH PROCESSING COMPLETE!")
+    print("="*50)
+
+if __name__ == "__main__":
+    main()

scripts/test_shenron_model.py

@@ -0,0 +1,98 @@
+import sys
+import os
+import numpy as np
+import time
+
+# Add project root and ISOLATE paths to sys.path
+project_root = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
+sys.path.append(project_root)
+
+# Import the model wrapper
+try:
+    from scripts.ISOLATE.model_wrapper import ShenronRadarModel
+    print(">>> Successfully imported ShenronRadarModel")
+except ImportError as e:
+    print(f">>> Failed to import ShenronRadarModel: {e}")
+    sys.exit(1)
+
+def run_smoke_test():
+    print("\n" + "="*50)
+    print("SHENRON MODEL SMOKE TEST")
+    print("="*50)
+    
+    success = True
+    
+    try:
+        # 1. Initialize the Model
+        print("\n[Step 1] Initializing ShenronRadarModel...")
+        start_time = time.time()
+        model = ShenronRadarModel(radar_type='radarbook')
+        print(f"Done in {time.time() - start_time:.2f}s")
+        
+        # 2. Prepare Synthetic Semantic LiDAR Data
+        # Format: [x, y, z, intensity, cos_inc_angle, object_idx, semantic_tag]
+        print("\n[Step 2] Preparing synthetic semantic LiDAR data...")
+        # Create a "vehicle" (tag 10) 15 meters ahead, slightly to the left
+        num_points = 100
+        data = np.zeros((num_points, 7))
+        
+        # Geometry
+        data[:, 0] = np.random.uniform(14.5, 15.5, num_points) # x (forward)
+        data[:, 1] = np.random.uniform(-2.0, -1.0, num_points) # y (left)
+        data[:, 2] = np.random.uniform(0.5, 1.5, num_points)   # z (up)
+        
+        # Physical properties
+        data[:, 3] = 0.5   # intensity
+        data[:, 4] = 0.9   # cos_inc_angle
+        data[:, 5] = 123   # object_idx
+        data[:, 6] = 10    # semantic_tag (Vehicle)
+        
+        print(f"Created {num_points} points representing a vehicle at 15m.")
+
+        # 3. Process data through the model
+        print("\n[Step 3] Running Shenron Radar Simulation (FMCW + DSP)...")
+        start_time = time.time()
+        rich_pcd = model.process(data)
+        process_time = time.time() - start_time
+        print(f"Done in {process_time:.2f}s")
+        
+        # 4. Validate output
+        print("\n[Step 4] Validating output...")
+        print(f"Output shape: {rich_pcd.shape}")
+        
+        if rich_pcd.shape[0] > 0:
+            print(f"Detected {rich_pcd.shape[0]} radar targets.")
+            print("Format: [x, y, z, velocity, magnitude]")
+            # Check for expected coordinates (roughly 15m ahead)
+            avg_x = np.mean(rich_pcd[:, 0])
+            avg_y = np.mean(rich_pcd[:, 1])
+            print(f"Average Detection Position: x={avg_x:.2f}, y={avg_y:.2f}")
+            
+            if 10 < avg_x < 20:
+                print("SUCCESS: Detections are in the expected range.")
+            else:
+                print("WARNING: Detections are outside expected range.")
+                
+            print("\nSample Detections:")
+            print(rich_pcd[:5])
+        else:
+            print("ERROR: No detections found from valid input data.")
+            success = False
+            
+    except Exception as e:
+        print(f"\n!!! CRITICAL ERROR during smoke test: {e}")
+        import traceback
+        traceback.print_exc()
+        success = False
+
+    print("\n" + "="*50)
+    if success:
+        print("RESULT: SMOKE TEST PASSED")
+    else:
+        print("RESULT: SMOKE TEST FAILED")
+    print("="*50)
+    
+    return success
+
+if __name__ == "__main__":
+    run_smoke_test()

src/recorder.py

@@ -79,9 +79,14 @@ class Recorder:
         radar_path = os.path.join(self.radar_path, radar_file)
         np.save(radar_path, radar_np)
 
-        # -------- LIDAR --------
+        # -------- LIDAR (Semantic) --------
+        # Dynamic reshape to handle semantic columns: [x, y, z, cos, obj, tag]
         lidar_data = np.frombuffer(lidar.raw_data, dtype=np.float32)
-        lidar_data = np.reshape(lidar_data, (-1, 4))  # x, y, z, intensity
+        total_points = len(lidar)
+        if total_points > 0:
+            lidar_data = np.reshape(lidar_data, (total_points, -1))
+        else:
+            lidar_data = np.empty((0, 6)) # Default to 6 columns for semantic
 
         lidar_file = f"frame_{self.frame_id:06d}.npy"
         lidar_path = os.path.join(self.lidar_path, lidar_file)

src/sensors.py

@@ -86,7 +86,7 @@ class SensorManager:
 
     # ---------------- LIDAR ----------------
     def _spawn_lidar(self):
-        lidar_bp = self.bp_lib.find("sensor.lidar.ray_cast")
+        lidar_bp = self.bp_lib.find("sensor.lidar.ray_cast_semantic")
 
         lidar_bp.set_attribute("range", str(config.LIDAR_RANGE))
         lidar_bp.set_attribute("channels", str(config.LIDAR_CHANNELS))