diff --git a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/lidar.py b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/lidar.py index 32a1098..941e275 100644 --- a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/lidar.py +++ b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/lidar.py @@ -90,15 +90,17 @@ def Cropped_forRadar(pc, veh_coord, veh_angle, radarobj): # skew_pc = np.vstack(((skew_pc ).T, pc[:, 3], pc[:, 5])).T #x,y,z,speed,material skew_pc = np.vstack(((skew_pc ).T, pc[:, 3], pc[:, 5],pc[:,6])).T #x,y,z,speed,material, cosines - # Restoration: X is Side (Index 0), Y is Forward (Index 1) - rowy = np.where((skew_pc[:, 1] > 0.5)) # Start 0.5m forward to avoid bumper + # NORMALIZATION: The points are now relative to the bumper (X=2.0). + # Any point with Y < 0.2 is the car's own hood or internal bumper structure. + rowy = np.where((skew_pc[:, 1] > 0.2)) new_pc = skew_pc[rowy, :].squeeze(0) new_pc = new_pc[new_pc[:,4]!=0] # Drop empty materials new_pc = new_pc[(new_pc[:,0]<30)*(new_pc[:,0]>-30)] # Side range (X) is +-30m new_pc = new_pc[(new_pc[:,1]<120)] # Forward range (Y) extended to 120m for headroom - new_pc = new_pc[(new_pc[:,2]<10)] # Height cut-off + new_pc = new_pc[(new_pc[:,2]<10)] # Height cut-off (Up) + new_pc = new_pc[(new_pc[:,2]>-2.2)] # Ground-level cut-off (30cm road-clearance to keep wheels/bumpers) simobj = Sceneset(new_pc) @@ -158,10 +160,11 @@ def run_lidar(sim_config, sem_lidar_frame): test = new_map_material(load_pc) # LEGACY REQUIREMENT: Shenron physics engine expects Index 1 to be Forward. - # We swap CARLA's X (Forward) into Index 1 and CARLA's Y (Right) into Index 0. + # MOUNT OFFSET: Radar is at +2.0m (Front Bumper), LiDAR is at +0.0m (Center Roof). + # We subtract 2.0m from the CARLA X (Forward) to normalize points to the Radar Center. points = np.zeros((np.shape(test)[0], 7)) points[:, 0] = test[:, 1] # Side (Y in CARLA) - points[:, 1] = test[:, 0] # Forward (X in CARLA) + points[:, 1] = test[:, 0] - 2.0 # Normalized Forward (X in CARLA) points[:, 2] = test[:, 2] # Up (Z in CARLA) """ diff --git a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/lidar_utils.py b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/lidar_utils.py index ace700e..e696a68 100644 --- a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/lidar_utils.py +++ b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/lidar_utils.py @@ -142,9 +142,9 @@ def new_map_material(test): unlabel_roughness = 0 wood_roughness = 0.0017 #1.7mm - conc_roughness = 0.0017 #1.7mm + conc_roughness = 0.01 # 1cm (Increased from 1.7mm for stability) human_roughness = 0.0001 # 100um - metal_roughness = 0.0001 # 100um + metal_roughness = 0.02 # 2cm (Increased from 100um to prevent NPC disappearance on turns) roughness = np.array([unlabel_roughness,wood_roughness,conc_roughness,human_roughness,metal_roughness]) diff --git a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/shenron/Sceneset.py b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/shenron/Sceneset.py index 399e853..4ca8f33 100644 --- a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/shenron/Sceneset.py +++ b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/shenron/Sceneset.py @@ -407,7 +407,7 @@ def get_loss_3(points, rho, elev_angle, angles, radar, use_spec = True, use_diff tx_dist_loss_exponent = 2 rx_dist_loss_exponent = 0 - spec_angle_thresh = 2*np.pi/180#*(1/rho) + spec_angle_thresh = 5.0*np.pi/180 # Increased from 2.0 to 5.0 for stability on turns P_incident = np.power(rho,2) * np.sin(elev_angle) * voxel_phi * voxel_theta * (1/np.power(rho,tx_dist_loss_exponent)) * K_sq diff --git a/scripts/ISOLATE/sim_radar_utils/radar_processor.py b/scripts/ISOLATE/sim_radar_utils/radar_processor.py index 19ac25b..0b15b05 100644 --- a/scripts/ISOLATE/sim_radar_utils/radar_processor.py +++ b/scripts/ISOLATE/sim_radar_utils/radar_processor.py @@ -22,12 +22,10 @@ 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.tile(signal.windows.blackmanharris(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.tile(signal.windows.blackmanharris(radarCfg['Np']), (radarCfg['N'], radarCfg['NrChn'], 1)) self.velWin = np.transpose(self.velWin, (0, 2, 1)) rangeRes = fftCfg['c0'] / (2*(radarCfg['fStop'] - radarCfg['fStrt'])) diff --git a/scripts/test_shenron.py b/scripts/test_shenron.py index c84da6f..7487a76 100644 --- a/scripts/test_shenron.py +++ b/scripts/test_shenron.py @@ -81,7 +81,7 @@ def run_testbench(iter_name): return iter_dir.mkdir(parents=True, exist_ok=True) - radar_types = ['awrl1432', 'radarbook', 'ti_cascade'] + radar_types = ['awrl1432', 'radarbook'] print(f"\n======================================") print(f"SHENRON TESTBENCH ITERATION: {iter_name}")