From 12effc5d7de4cc221699656f98bc7e62c88d44e7 Mon Sep 17 00:00:00 2001 From: Daniel Santos-Olivan Date: Thu, 26 Nov 2020 16:39:03 +0100 Subject: [PATCH] Updated raycast determinism script --- .../util/check_raycast_sensors_determinism.py | 314 ++++++++++-------- 1 file changed, 168 insertions(+), 146 deletions(-) diff --git a/PythonAPI/util/check_raycast_sensors_determinism.py b/PythonAPI/util/check_raycast_sensors_determinism.py index a292caefe..16bdefacb 100644 --- a/PythonAPI/util/check_raycast_sensors_determinism.py +++ b/PythonAPI/util/check_raycast_sensors_determinism.py @@ -36,27 +36,42 @@ import carla class Scenario(): - def __init__(self, client, world): + def __init__(self, client, world, save_snapshots_mode=False): self.world = world self.client = client self.actor_list = [] - self.sensor_list = [] self.init_timestamp = [] - self.sensor_queue = Queue() self.active = False self.prefix = "" - - def init_scene(self, prefix): - self.prefix = prefix - self.actor_list = [] + self.save_snapshots_mode = save_snapshots_mode + self.snapshots = [] self.sensor_list = [] self.sensor_queue = Queue() + + def init_scene(self, prefix, settings = None, spectator_tr = None): + self.prefix = prefix + self.actor_list = [] self.active = True + self.snapshots = [] + self.sensor_list = [] + self.sensor_queue = Queue() + + self.reload_world(settings, spectator_tr) # Init timestamp snapshot = self.world.get_snapshot() self.init_timestamp = {'frame0' : snapshot.frame, 'time0' : snapshot.timestamp.elapsed_seconds} + def add_actor(self, actor, actor_name="Actor"): + actor_idx = len(self.actor_list) + + name = str(actor_idx) + "_" + actor_name + + self.actor_list.append((name, actor)) + + if self.save_snapshots_mode: + self.snapshots.append(np.empty((0,11), float)) + def wait(self, frames=100): for _i in range(0, frames): self.world.tick() @@ -64,8 +79,78 @@ class Scenario(): for _s in self.sensor_list: self.sensor_queue.get(True, 1.0) - def add_sensor(self, sensor, sensor_type): + def clear_scene(self): + for sensor in self.sensor_list: + sensor[1].destroy() + for actor in self.actor_list: + actor.destroy() + + self.active = False + + def reload_world(self, settings = None, spectator_tr = None): + self.client.reload_world() + if settings is not None: + self.world.apply_settings(settings) + if spectator_tr is not None: + self.reset_spectator(spectator_tr) + + def reset_spectator(self, spectator_tr): + spectator = self.world.get_spectator() + spectator.set_transform(spectator_tr) + + def save_snapshot(self, actor): + snapshot = self.world.get_snapshot() + + actor_snapshot = np.array([ + float(snapshot.frame - self.init_timestamp['frame0']), \ + snapshot.timestamp.elapsed_seconds - self.init_timestamp['time0'], \ + actor.get_velocity().x, actor.get_velocity().y, actor.get_velocity().z, \ + actor.get_location().x, actor.get_location().y, actor.get_location().z, \ + actor.get_angular_velocity().x, actor.get_angular_velocity().y, actor.get_angular_velocity().z]) + return actor_snapshot + + def save_snapshots(self): + if not self.save_snapshots_mode: + return + + for i in range (0, len(self.actor_list)): + self.snapshots[i] = np.vstack((self.snapshots[i], self.save_snapshot(self.actor_list[i][1]))) + + def save_snapshots_to_disk(self): + if not self.save_snapshots_mode: + return + + for i, actor in enumerate(self.actor_list): + np.savetxt(self.get_filename(actor[0]), self.snapshots[i]) + + def get_filename_with_prefix(self, prefix, actor_id=None, frame=None): + add_id = "" if actor_id is None else "_" + actor_id + add_frame = "" if frame is None else ("_%04d") % frame + return prefix + add_id + add_frame + ".out" + + def get_filename(self, actor_id=None, frame=None): + return self.get_filename_with_prefix(self.prefix, actor_id, frame) + + def run_simulation(self, prefix, run_settings, spectator_tr, tics = 200): + original_settings = self.world.get_settings() + + self.init_scene(prefix, run_settings, spectator_tr) + + t_start = time.perf_counter() + for _i in range(0, tics): + self.world.tick() + self.save_snapshots() + self.sensor_syncronization() + t_end = time.perf_counter() + + self.world.apply_settings(original_settings) + self.save_snapshots_to_disk() + self.clear_scene() + + return t_end - t_start + + def add_sensor(self, sensor, sensor_type): sen_idx = len(self.sensor_list) if sensor_type == "LiDAR": name = str(sen_idx) + "_LiDAR" @@ -79,7 +164,6 @@ class Scenario(): self.sensor_list.append((name, sensor)) - def add_lidar_snapshot(self, lidar_data, name="LiDAR"): if not self.active: return @@ -88,10 +172,7 @@ class Scenario(): points = np.reshape(points, (int(points.shape[0] / 4), 4)) frame = lidar_data.frame - self.init_timestamp['frame0'] - time_f = lidar_data.timestamp - self.init_timestamp['time0'] - np.savetxt(self.get_filename(name, frame), points) - self.sensor_queue.put((lidar_data.frame, name)) def add_semlidar_snapshot(self, lidar_data, name="SemLiDAR"): @@ -101,14 +182,10 @@ class Scenario(): data = np.frombuffer(lidar_data.raw_data, dtype=np.dtype([ ('x', np.float32), ('y', np.float32), ('z', np.float32), ('CosAngle', np.float32), ('ObjIdx', np.uint32), ('ObjTag', np.uint32)])) - points = np.array([data['x'], data['y'], data['z'], data['CosAngle'], data['ObjTag']]).T frame = lidar_data.frame - self.init_timestamp['frame0'] - time_f = lidar_data.timestamp - self.init_timestamp['time0'] - np.savetxt(self.get_filename(name, frame), points) - self.sensor_queue.put((lidar_data.frame, name)) def add_radar_snapshot(self, radar_data, name="Radar"): @@ -119,66 +196,35 @@ class Scenario(): points = np.reshape(points, (int(points.shape[0] / 4), 4)) frame = radar_data.frame - self.init_timestamp['frame0'] - time_f = radar_data.timestamp - self.init_timestamp['time0'] - np.savetxt(self.get_filename(name, frame), points) - self.sensor_queue.put((radar_data.frame, name)) - def clear_scene(self): + def sensor_syncronization(self): + # Sensor Syncronization + w_frame = self.world.get_snapshot().frame for sensor in self.sensor_list: - sensor[1].destroy() - - for actor in self.actor_list: - actor.destroy() - - self.active = False - - def get_filename_with_prefix(self, prefix, actor_id=None, frame=None): - add_id = "" if actor_id is None else "_" + actor_id - add_frame = "" if frame is None else ("_%04d") % frame - return prefix + add_id + add_frame + ".out" - - def get_filename(self, actor_id=None, frame=None): - return self.get_filename_with_prefix(self.prefix, actor_id, frame) - - def run_simulation(self, prefix, tics = 200): - self.init_scene(prefix) - - for _i in range(0, tics): - self.world.tick() - w_frame = self.world.get_snapshot().frame - - for s in self.sensor_list: - s_frame = self.sensor_queue.get(True, 1.0)[0] - - if w_frame != s_frame: - print("Error!!! frames are not equal for %d: %d %d" % (s[0], w_frame, s_frame)) - - self.clear_scene() + s_frame = self.sensor_queue.get(True, 1.0)[0] + if w_frame != s_frame: + print("Error!!! frames are not equal for %s: %d %d" % (sensor[0], w_frame, s_frame)) class SpawnLidarNoDropff(Scenario): - def init_scene(self, prefix): - super().init_scene(prefix) + def init_scene(self, prefix, settings = None, spectator_tr = None): + super().init_scene(prefix, settings, spectator_tr) - world = self.world - - blueprint_library = world.get_blueprint_library() + blueprint_library = self.world.get_blueprint_library() vehicle00_tr = carla.Transform(carla.Location(140, -205, 0.1), carla.Rotation(yaw=181.5)) - vehicle00 = world.spawn_actor(blueprint_library.filter("tt")[0], vehicle00_tr) + vehicle00 = self.world.spawn_actor(blueprint_library.filter("tt")[0], vehicle00_tr) vehicle00.set_target_velocity(carla.Vector3D(-25, 0, 0)) - lidar_bp = world.get_blueprint_library().find('sensor.lidar.ray_cast') - lidar_bp.set_attribute('channels', '64') - lidar_bp.set_attribute('points_per_second', '200000') + lidar_bp = self.world.get_blueprint_library().find('sensor.lidar.ray_cast') lidar_bp.set_attribute('dropoff_general_rate', '0.0') lidar_bp.set_attribute('noise_seed', '43233') lidar_tr = carla.Transform(carla.Location(z=2)) - lidar = world.spawn_actor(lidar_bp, lidar_tr, attach_to=vehicle00) + lidar = self.world.spawn_actor(lidar_bp, lidar_tr, attach_to=vehicle00) self.add_sensor(lidar, "LiDAR") self.actor_list.append(vehicle00) @@ -186,24 +232,20 @@ class SpawnLidarNoDropff(Scenario): self.wait(1) class SpawnSemanticLidar(Scenario): - def init_scene(self, prefix): - super().init_scene(prefix) + def init_scene(self, prefix, settings = None, spectator_tr = None): + super().init_scene(prefix, settings, spectator_tr) - world = self.world - - blueprint_library = world.get_blueprint_library() + blueprint_library = self.world.get_blueprint_library() vehicle00_tr = carla.Transform(carla.Location(140, -205, 0.1), carla.Rotation(yaw=181.5)) - vehicle00 = world.spawn_actor(blueprint_library.filter("tt")[0], vehicle00_tr) + vehicle00 = self.world.spawn_actor(blueprint_library.filter("tt")[0], vehicle00_tr) vehicle00.set_target_velocity(carla.Vector3D(-25, 0, 0)) - lidar_bp = world.get_blueprint_library().find('sensor.lidar.ray_cast_semantic') - lidar_bp.set_attribute('channels', '64') - lidar_bp.set_attribute('points_per_second', '200000') + lidar_bp = self.world.get_blueprint_library().find('sensor.lidar.ray_cast_semantic') lidar_tr = carla.Transform(carla.Location(z=2)) - lidar = world.spawn_actor(lidar_bp, lidar_tr, attach_to=vehicle00) + lidar = self.world.spawn_actor(lidar_bp, lidar_tr, attach_to=vehicle00) self.add_sensor(lidar, "SemLiDAR") self.actor_list.append(vehicle00) @@ -211,24 +253,21 @@ class SpawnSemanticLidar(Scenario): self.wait(1) class SpawnRadar(Scenario): + def init_scene(self, prefix, settings = None, spectator_tr = None): + super().init_scene(prefix, settings, spectator_tr) - def init_scene(self, prefix): - super().init_scene(prefix) - - world = self.world - - blueprint_library = world.get_blueprint_library() + blueprint_library = self.world.get_blueprint_library() vehicle00_tr = carla.Transform(carla.Location(140, -205, 0.1), carla.Rotation(yaw=181.5)) - vehicle00 = world.spawn_actor(blueprint_library.filter("tt")[0], vehicle00_tr) + vehicle00 = self.world.spawn_actor(blueprint_library.filter("tt")[0], vehicle00_tr) vehicle00.set_target_velocity(carla.Vector3D(-25, 0, 0)) - radar_bp = world.get_blueprint_library().find('sensor.other.radar') + radar_bp = self.world.get_blueprint_library().find('sensor.other.radar') radar_bp.set_attribute('noise_seed', '54283') radar_tr = carla.Transform(carla.Location(z=2)) - radar = world.spawn_actor(radar_bp, radar_tr, attach_to=vehicle00) + radar = self.world.spawn_actor(radar_bp, radar_tr, attach_to=vehicle00) self.add_sensor(radar, "Radar") self.actor_list.append(vehicle00) @@ -236,25 +275,22 @@ class SpawnRadar(Scenario): self.wait(1) class SpawnLidarWithDropff(Scenario): - def init_scene(self, prefix): - super().init_scene(prefix) + def init_scene(self, prefix, settings = None, spectator_tr = None): + super().init_scene(prefix, settings, spectator_tr) - world = self.world - - blueprint_library = world.get_blueprint_library() + blueprint_library = self.world.get_blueprint_library() vehicle00_tr = carla.Transform(carla.Location(140, -205, 0.1), carla.Rotation(yaw=181.5)) - vehicle00 = world.spawn_actor(blueprint_library.filter("tt")[0], vehicle00_tr) + vehicle00 = self.world.spawn_actor(blueprint_library.filter("tt")[0], vehicle00_tr) vehicle00.set_target_velocity(carla.Vector3D(-25, 0, 0)) - lidar_bp = world.get_blueprint_library().find('sensor.lidar.ray_cast') + lidar_bp = self.world.get_blueprint_library().find('sensor.lidar.ray_cast') lidar_bp.set_attribute('channels', '64') - lidar_bp.set_attribute('points_per_second', '200000') lidar_bp.set_attribute('noise_seed', '249013') lidar_tr = carla.Transform(carla.Location(z=2)) - lidar = world.spawn_actor(lidar_bp, lidar_tr, attach_to=vehicle00) + lidar = self.world.spawn_actor(lidar_bp, lidar_tr, attach_to=vehicle00) self.add_sensor(lidar, "LiDAR") self.actor_list.append(vehicle00) @@ -262,40 +298,37 @@ class SpawnLidarWithDropff(Scenario): self.wait(1) class SpawnAllRaycastSensors(Scenario): + def init_scene(self, prefix, settings = None, spectator_tr = None): + super().init_scene(prefix, settings, spectator_tr) - def init_scene(self, prefix): - super().init_scene(prefix) - - world = self.world - - blueprint_library = world.get_blueprint_library() + blueprint_library = self.world.get_blueprint_library() vehicle00_tr = carla.Transform(carla.Location(140, -205, 0.1), carla.Rotation(yaw=181.5)) - vehicle00 = world.spawn_actor(blueprint_library.filter("tt")[0], vehicle00_tr) + vehicle00 = self.world.spawn_actor(blueprint_library.filter("tt")[0], vehicle00_tr) vehicle00.set_target_velocity(carla.Vector3D(-25, 0, 0)) - vehicle01_tr = carla.Transform(carla.Location(50, -200, 0.1), carla.Rotation(yaw=-178.5)) - vehicle01 = world.spawn_actor(blueprint_library.filter("lincoln")[0], vehicle01_tr) - vehicle01.set_target_velocity(carla.Vector3D(+25, 0, 0)) + vehicle01_tr = carla.Transform(carla.Location(50, -200, 0.1), carla.Rotation(yaw=1.5)) + vehicle01 = self.world.spawn_actor(blueprint_library.filter("lincoln")[0], vehicle01_tr) + vehicle01.set_target_velocity(carla.Vector3D(25, 0, 0)) - radar_bp = world.get_blueprint_library().find('sensor.other.radar') + radar_bp = self.world.get_blueprint_library().find('sensor.other.radar') radar_bp.set_attribute('noise_seed', '54283') radar_tr = carla.Transform(carla.Location(z=2)) - radar = world.spawn_actor(radar_bp, radar_tr, attach_to=vehicle00) + radar = self.world.spawn_actor(radar_bp, radar_tr, attach_to=vehicle00) - lidar01_bp = world.get_blueprint_library().find('sensor.lidar.ray_cast') + lidar01_bp = self.world.get_blueprint_library().find('sensor.lidar.ray_cast') lidar01_bp.set_attribute('noise_seed', '12134') lidar01_tr = carla.Transform(carla.Location(x=1, z=2)) - lidar01 = world.spawn_actor(lidar01_bp, lidar01_tr, attach_to=vehicle00) + lidar01 = self.world.spawn_actor(lidar01_bp, lidar01_tr, attach_to=vehicle00) - lidar02_bp = world.get_blueprint_library().find('sensor.lidar.ray_cast_semantic') + lidar02_bp = self.world.get_blueprint_library().find('sensor.lidar.ray_cast_semantic') lidar02_tr = carla.Transform(carla.Location(x=1, z=2)) - lidar02 = world.spawn_actor(lidar02_bp, lidar02_tr, attach_to=vehicle01) + lidar02 = self.world.spawn_actor(lidar02_bp, lidar02_tr, attach_to=vehicle01) - lidar03_bp = world.get_blueprint_library().find('sensor.lidar.ray_cast') + lidar03_bp = self.world.get_blueprint_library().find('sensor.lidar.ray_cast') lidar03_bp.set_attribute('noise_seed', '23135') lidar03_tr = carla.Transform(carla.Location(z=2)) - lidar03 = world.spawn_actor(lidar03_bp, lidar03_tr, attach_to=vehicle01) + lidar03 = self.world.spawn_actor(lidar03_bp, lidar03_tr, attach_to=vehicle01) self.add_sensor(radar, "Radar") self.add_sensor(lidar01, "LiDAR") @@ -306,7 +339,7 @@ class SpawnAllRaycastSensors(Scenario): self.wait(1) -class TestScenario(): +class TestSensorScenario(): def __init__(self, scene, output_path): self.scene = scene self.world = self.scene.world @@ -363,44 +396,41 @@ class TestScenario(): return determinism_set def test_scenario(self, repetitions = 1, sim_tics = 100): - print("Testing Determinism in %s -> " % \ - (self.scenario_name), end='') - - settings = self.world.get_settings() - settings.synchronous_mode = True - settings.fixed_delta_seconds = 0.05 - self.world.apply_settings(settings) + output_str = "Testing Determinism in %s -> " % (self.scenario_name) prefix = self.output_path + self.scenario_name - t_start = time.perf_counter() + config_settings = self.world.get_settings() + config_settings.synchronous_mode = True + config_settings.fixed_delta_seconds = 0.05 + + spectator_tr = carla.Transform(carla.Location(160, -205, 10), carla.Rotation(yaw=180)) + sim_prefixes = [] + t_comp = 0 for i in range(0, repetitions): prefix_rep = prefix + "_rep_" + ("%03d" % i) - self.scene.run_simulation(prefix_rep, tics=sim_tics) + t_comp += self.scene.run_simulation(prefix_rep, config_settings, spectator_tr, tics=sim_tics) sim_prefixes.append(prefix_rep) - t_end = time.perf_counter() - determ_repet = self.check_simulations(sim_prefixes, sim_tics) - print("Deterministic Repetitions: %r / %2d" % (determ_repet, repetitions), end="") - print(" -> Comp. FPS: %.0f" % ((repetitions*sim_tics)/(t_end-t_start))) - return + output_str += "Deterministic Repetitions: %r / %2d" % (determ_repet, repetitions) + output_str += " -> Comp. FPS: %.0f" % ((repetitions*sim_tics)/t_comp) + + if determ_repet == repetitions: + print("Error!!! Scenario %s is not deterministic: %d / %d" % (self.scenario_name, determ_repet, repetitions)) + + return output_str def main(arg): """Main function of the script""" client = carla.Client(arg.host, arg.port) - client.set_timeout(2.0) + client.set_timeout(5.0) world = client.get_world() pre_settings = world.get_settings() world.apply_settings(pre_settings) - spectator_transform = carla.Transform(carla.Location(160, -205, 5), carla.Rotation(yaw=180)) - spectator_transform.location.z += 5 - spectator = world.get_spectator() - spectator.set_transform(spectator_transform) - try: # Setting output temporal folder output_path = os.path.dirname(os.path.realpath(__file__)) @@ -408,27 +438,19 @@ def main(arg): if not os.path.exists(output_path): os.mkdir(output_path) + test_list = [ + TestSensorScenario(SpawnAllRaycastSensors(client, world), output_path), + TestSensorScenario(SpawnLidarNoDropff(client, world), output_path), + TestSensorScenario(SpawnLidarWithDropff(client, world), output_path), + TestSensorScenario(SpawnSemanticLidar(client, world), output_path), + TestSensorScenario(SpawnRadar(client, world), output_path) + ] + repetitions = 10 - - print("--------------------------------------------------------------") - test_all = TestScenario(SpawnAllRaycastSensors(client, world), output_path) - test_all.test_scenario(repetitions) - - print("--------------------------------------------------------------") - test_lidar_1 = TestScenario(SpawnLidarNoDropff(client, world), output_path) - test_lidar_1.test_scenario(repetitions) - - print("--------------------------------------------------------------") - test_lidar_2 = TestScenario(SpawnLidarWithDropff(client, world), output_path) - test_lidar_2.test_scenario(repetitions) - - print("--------------------------------------------------------------") - test_semlidar = TestScenario(SpawnSemanticLidar(client, world), output_path) - test_semlidar.test_scenario(repetitions) - - print("--------------------------------------------------------------") - test_radar = TestScenario(SpawnRadar(client, world), output_path) - test_radar.test_scenario(repetitions) + for item in test_list: + print("--------------------------------------------------------------") + out = item.test_scenario(repetitions) + print(out) print("--------------------------------------------------------------")