Updated raycast determinism script

This commit is contained in:
Daniel Santos-Olivan 2020-11-26 16:39:03 +01:00 committed by bernat
parent 2d36060e52
commit 12effc5d7d
1 changed files with 168 additions and 146 deletions

View File

@ -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("--------------------------------------------------------------")