Added script for check sensor determinsm
This commit is contained in:
Daniel Santos-Olivan
Marc Garcia Puig
parent
ea194c462e
commit
db64712a53
|
|
@ -0,0 +1,388 @@
|
|||
#!/usr/bin/env python
|
||||
|
||||
# Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma de
|
||||
# Barcelona (UAB).
|
||||
#
|
||||
# This work is licensed under the terms of the MIT license.
|
||||
# For a copy, see <https://opensource.org/licenses/MIT>.
|
||||
|
||||
"""
|
||||
Check raycast sensors determinism for CARLA
|
||||
This script spawn all the raycast sensors in a simple scenario and check if their
|
||||
output are deterministic.
|
||||
"""
|
||||
|
||||
import glob
|
||||
import os
|
||||
import sys
|
||||
import argparse
|
||||
import time
|
||||
import filecmp
|
||||
import shutil
|
||||
from queue import Queue
|
||||
from queue import Empty
|
||||
|
||||
import numpy as np
|
||||
|
||||
try:
|
||||
sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % (
|
||||
sys.version_info.major,
|
||||
sys.version_info.minor,
|
||||
'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
|
||||
except IndexError:
|
||||
pass
|
||||
|
||||
import carla
|
||||
|
||||
|
||||
class Scenario():
|
||||
def __init__(self, client, world):
|
||||
self.world = world
|
||||
self.client = client
|
||||
self.actor_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.sensor_queue = Queue()
|
||||
self.active = True
|
||||
|
||||
snapshot = self.world.get_snapshot()
|
||||
self.init_timestamp = {'frame0' : snapshot.frame, 'time0' :
|
||||
snapshot.timestamp.elapsed_seconds}
|
||||
|
||||
def wait(self, frames=100):
|
||||
for _i in range(0, frames):
|
||||
self.world.tick()
|
||||
if self.active:
|
||||
self.sensor_queue.get(True, 1.0)
|
||||
|
||||
def add_lidar_snapshot(self, lidar_data):
|
||||
if not self.active:
|
||||
return
|
||||
|
||||
points = np.frombuffer(lidar_data.raw_data, dtype=np.dtype('f4'))
|
||||
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(frame), points)
|
||||
|
||||
self.sensor_queue.put((lidar_data.frame, "LiDAR"))
|
||||
|
||||
def add_semlidar_snapshot(self, lidar_data):
|
||||
if not self.active:
|
||||
return
|
||||
|
||||
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(frame), points)
|
||||
|
||||
self.sensor_queue.put((lidar_data.frame, "SemLiDAR"))
|
||||
|
||||
def add_radar_snapshot(self, radar_data):
|
||||
if not self.active:
|
||||
return
|
||||
|
||||
points = np.frombuffer(radar_data.raw_data, dtype=np.dtype('f4'))
|
||||
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(frame), points)
|
||||
|
||||
self.sensor_queue.put((radar_data.frame, "Radar"))
|
||||
|
||||
def clear_scene(self):
|
||||
for actor in self.actor_list:
|
||||
actor.destroy()
|
||||
|
||||
self.active = False
|
||||
|
||||
def get_filename_with_prefix(self, prefix, f):
|
||||
return prefix + ("%04d") % f + ".out"
|
||||
|
||||
def get_filename(self, f):
|
||||
return self.prefix + ("%04d") % f + ".out"
|
||||
|
||||
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
|
||||
s_frame = self.sensor_queue.get(True, 1.0)[0]
|
||||
|
||||
if w_frame != s_frame:
|
||||
print("Error!!! frame are not equal: %d %d" % (w_frame, s_frame))
|
||||
|
||||
self.clear_scene()
|
||||
self.wait(10)
|
||||
|
||||
|
||||
class SpawnLidarNoDropff(Scenario):
|
||||
def init_scene(self, prefix):
|
||||
super().init_scene(prefix)
|
||||
|
||||
world = self.world
|
||||
|
||||
blueprint_library = 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.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.set_attribute('dropoff_general_rate', '0.0')
|
||||
|
||||
lidar_tr = carla.Transform(carla.Location(z=2))
|
||||
lidar = world.spawn_actor(lidar_bp, lidar_tr, attach_to=vehicle00)
|
||||
|
||||
lidar.listen(self.add_lidar_snapshot)
|
||||
|
||||
self.actor_list.append(lidar)
|
||||
self.actor_list.append(vehicle00)
|
||||
|
||||
self.wait(1)
|
||||
|
||||
class SpawnSemanticLidar(Scenario):
|
||||
def init_scene(self, prefix):
|
||||
super().init_scene(prefix)
|
||||
|
||||
world = self.world
|
||||
|
||||
blueprint_library = 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.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_tr = carla.Transform(carla.Location(z=2))
|
||||
lidar = world.spawn_actor(lidar_bp, lidar_tr, attach_to=vehicle00)
|
||||
|
||||
lidar.listen(self.add_semlidar_snapshot)
|
||||
|
||||
self.actor_list.append(lidar)
|
||||
self.actor_list.append(vehicle00)
|
||||
|
||||
self.wait(1)
|
||||
|
||||
class SpawnRadar(Scenario):
|
||||
|
||||
def init_scene(self, prefix):
|
||||
super().init_scene(prefix)
|
||||
|
||||
world = self.world
|
||||
|
||||
blueprint_library = 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.set_target_velocity(carla.Vector3D(-25, 0, 0))
|
||||
|
||||
radar_bp = world.get_blueprint_library().find('sensor.other.radar')
|
||||
|
||||
radar_tr = carla.Transform(carla.Location(z=2))
|
||||
radar = world.spawn_actor(radar_bp, radar_tr, attach_to=vehicle00)
|
||||
|
||||
radar.listen(self.add_radar_snapshot)
|
||||
|
||||
self.actor_list.append(radar)
|
||||
self.actor_list.append(vehicle00)
|
||||
|
||||
self.wait(1)
|
||||
|
||||
class SpawnLidarWithDropff(Scenario):
|
||||
def init_scene(self, prefix):
|
||||
super().init_scene(prefix)
|
||||
|
||||
world = self.world
|
||||
|
||||
blueprint_library = 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.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_tr = carla.Transform(carla.Location(z=2))
|
||||
lidar = world.spawn_actor(lidar_bp, lidar_tr, attach_to=vehicle00)
|
||||
|
||||
lidar.listen(self.add_lidar_snapshot)
|
||||
|
||||
self.actor_list.append(lidar)
|
||||
self.actor_list.append(vehicle00)
|
||||
|
||||
self.wait(1)
|
||||
|
||||
|
||||
class TestScenario():
|
||||
def __init__(self, scene):
|
||||
self.scene = scene
|
||||
self.world = self.scene.world
|
||||
self.client = self.scene.client
|
||||
self.scenario_name = self.scene.__class__.__name__
|
||||
|
||||
def compare_files(self, file_i, file_j):
|
||||
|
||||
# First, we check if the files are exactly equal,
|
||||
# if they are the simulations are equivalent
|
||||
check_ij = filecmp.cmp(file_i, file_j)
|
||||
if check_ij:
|
||||
return True
|
||||
|
||||
# If not, if we have different number of points
|
||||
# the simulations are not equivalent
|
||||
data_i = np.loadtxt(file_i)
|
||||
data_j = np.loadtxt(file_j)
|
||||
if data_i.shape != data_j.shape:
|
||||
return False
|
||||
|
||||
# If they have the same number of points but there is
|
||||
# a small diference, the simulations could be equivalent
|
||||
# differing only in floaring-point arithmetic errors
|
||||
max_error = np.amax(np.abs(data_i-data_j))
|
||||
|
||||
return max_error < 0.01
|
||||
|
||||
def check_simulations(self, rep_prefixes, sim_tics):
|
||||
repetitions = len(rep_prefixes)
|
||||
mat_check = np.zeros((repetitions, repetitions), int)
|
||||
|
||||
for i in range(0, repetitions):
|
||||
mat_check[i][i] = 1
|
||||
for j in range(0, i):
|
||||
sim_check = True
|
||||
for f_idx in range(1, sim_tics):
|
||||
file_i = self.scene.get_filename_with_prefix(rep_prefixes[i], f_idx)
|
||||
file_j = self.scene.get_filename_with_prefix(rep_prefixes[j], f_idx)
|
||||
|
||||
check_ij = self.compare_files(file_i, file_j)
|
||||
sim_check = sim_check and check_ij
|
||||
|
||||
mat_check[i][j] = int(sim_check)
|
||||
mat_check[j][i] = int(sim_check)
|
||||
|
||||
determinism = np.sum(mat_check,axis=1)
|
||||
|
||||
determinism_set = list(set(determinism))
|
||||
determinism_set.sort(reverse=True)
|
||||
|
||||
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)
|
||||
|
||||
path = os.path.dirname(os.path.realpath(__file__))
|
||||
path = os.path.join(path, "_sensors") + os.path.sep
|
||||
if not os.path.exists(path):
|
||||
os.mkdir(path)
|
||||
|
||||
prefix = path + self.scenario_name
|
||||
|
||||
t_start = time.perf_counter()
|
||||
sim_prefixes = []
|
||||
for i in range(0, repetitions):
|
||||
prefix_rep = prefix + "_rep_" + ("%03d" % i) + "_"
|
||||
self.scene.run_simulation(prefix_rep, 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
|
||||
|
||||
|
||||
def main(arg):
|
||||
"""Main function of the script"""
|
||||
client = carla.Client(arg.host, arg.port)
|
||||
client.set_timeout(2.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:
|
||||
repetitions = 10
|
||||
test_lidar_1 = TestScenario(SpawnLidarNoDropff(client, world))
|
||||
test_lidar_1.test_scenario(repetitions)
|
||||
|
||||
test_lidar_2 = TestScenario(SpawnLidarWithDropff(client, world))
|
||||
test_lidar_2.test_scenario(repetitions)
|
||||
|
||||
test_semlidar = TestScenario(SpawnSemanticLidar(client, world))
|
||||
test_semlidar.test_scenario(repetitions)
|
||||
|
||||
test_radar = TestScenario(SpawnRadar(client, world))
|
||||
test_radar.test_scenario(repetitions)
|
||||
|
||||
finally:
|
||||
world.apply_settings(pre_settings)
|
||||
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
argparser = argparse.ArgumentParser(
|
||||
description=__doc__)
|
||||
argparser.add_argument(
|
||||
'--host',
|
||||
metavar='H',
|
||||
default='localhost',
|
||||
help='IP of the host CARLA Simulator (default: localhost)')
|
||||
argparser.add_argument(
|
||||
'-p', '--port',
|
||||
metavar='P',
|
||||
default=2000,
|
||||
type=int,
|
||||
help='TCP port of CARLA Simulator (default: 2000)')
|
||||
argparser.add_argument(
|
||||
'--filter',
|
||||
metavar='PATTERN',
|
||||
default='model3',
|
||||
help='actor filter (default: "vehicle.*")')
|
||||
args = argparser.parse_args()
|
||||
|
||||
try:
|
||||
main(args)
|
||||
except KeyboardInterrupt:
|
||||
print(' - Exited by user.')
|
||||
|
||||
Loading…
Reference in New Issue