diff --git a/PythonAPI/util/check_lidar_bb.py b/PythonAPI/util/check_lidar_bb.py new file mode 100644 index 000000000..316d44c48 --- /dev/null +++ b/PythonAPI/util/check_lidar_bb.py @@ -0,0 +1,371 @@ +#!/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 . + +""" + +Lidar/BB check for CARLA +This script obtains the LiDAR's point cloud corresponding to all the vehicles +of the scene and make sure that they are inside the bounding box of the +corresponding actor. +This is done in a predefined route in Town03 with a high speed and several agressive +turns. + +In a nutshell, the script have a queue that is filled in each frame with a lidar point +cloud and an structure for storing the Bounding Boxes. This last one is emulated as a +sensor filling the queue in the on_tick callback of the carla.world. In this way, we make +sure that we are correctly syncronizing the lidar point cloud and BB/actor transformations. +Then, we select the points corresponding to each actor (car) in the scene and check they +are inside the bounding boxes of that actor, all in each vehicle frame of reference. + +Important Data structure description: + + Lidar data structure: four element tuple with: + - [0] Frame + - [1] Sensor name: 'semlidar' + - [2] Point cloud in the form of a numpy dictionary with all semantic lidar information + - [3] Global transformation of the sensor + + Bounding box data structure: four element tuple with: + - [0] Frame + - [1] Sensor name: 'bb' + - [2] List of actor information: each a tuple with: + - [0] Actor id + - [1] Actor type (blueprint's name) + - [0] Actor's global transformation + - [0] Actor's bounding box + + ActorTrace class: Takes the Lidar data structure and one actor information and + check if all the data points related with this actor are inside its BB. + This is done in the local coordinate frame of the actor and should be done like: + trace = ActorTrace(actor_info, lidar_data) + trace.process() + trace.check_lidar_data() + + +""" + +import glob +import os +import sys +import numpy as np +from queue import Queue +from queue import Empty + +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 ActorTrace(object): + """Class that store and process information about an actor at certain moment.""" + def __init__(self, actor, lidar): + self.set_lidar(lidar) + self.set_actor(actor) + self._lidar_pc_local = np.array([]) + self._bb_vertices = np.array([]) + self._bb_minlimits = [0, 0, 0] + self._bb_maxlimits = [0, 0, 0] + + def set_lidar(self, lidar): + self._frame = lidar[0] + self._lidar_data = lidar[2] + self._lidar_transf = lidar[3] + + def set_actor(self, actor): + self._actor_id = actor[0] + self._actor_type = actor[1] + self._actor_transf = actor[2] + self._actor_bb = actor[3] + + def process(self): + # Filter lidar points that correspond to my actor id + data_actor = self._lidar_data[self._lidar_data['ObjIdx'] == self._actor_id] + + # Take the xyz point cloud data and transform it to actor's frame + points = np.array([data_actor['x'], data_actor['y'], data_actor['z']]).T + points = np.append(points, np.ones((points.shape[0], 1)), axis=1) + points = np.dot(self._lidar_transf.get_matrix(), points.T).T # sensor -> world + points = np.dot(self._actor_transf.get_inverse_matrix(), points.T).T # world -> actor + points = points[:, :-1] + + # Saving the points in 'local' coordinates + self._lidar_pc_local = points + + # We compute the limits in the local frame of reference using the + # vertices of the bounding box + vertices = self._actor_bb.get_local_vertices() + ver_py = [] + for v in vertices: + ver_py.append([v.x, v.y, v.z]) + ver_np = np.array(ver_py) + + self._bb_vertices = ver_np + + self._bb_minlimits = ver_np.min(axis=0) - 0.001 + self._bb_maxlimits = ver_np.max(axis=0) + 0.001 + + def print(self, print_if_empty = False): + if self._lidar_pc_local.shape[0] > 0 or print_if_empty: + np.savetxt("veh_data_%d_%s_%d.out" % (self._frame, self._actor_type, self._actor_id), self._lidar_pc_local) + np.savetxt("bb_data_%d_%s_%d.out" % (self._frame, self._actor_type, self._actor_id), self._bb_vertices) + + def lidar_is_outside_bb(self, check_axis = [True, True, True]): + lidar_pc = self._lidar_pc_local + + if check_axis[0]: + xmin = self._bb_minlimits[0] + xmax = self._bb_maxlimits[0] + out = np.any((lidar_pc[:,0] > xmax) | (lidar_pc[:,0] < xmin)) + if out: + print("Problem with x axis") + return True + + if check_axis[1]: + ymin = self._bb_minlimits[1] + ymax = self._bb_maxlimits[1] + out = np.any((lidar_pc[:, 1] > ymax) | (lidar_pc[:, 1] < ymin)) + if out: + print("Problem with y axis") + return True + + if check_axis[2]: + zmin = self._bb_minlimits[2] + zmax = self._bb_maxlimits[2] + out = np.any((lidar_pc[:, 2] > zmax) | (lidar_pc[:, 2] < zmin)) + if out: + print("Problem with z axis") + return True + + return False + + def check_lidar_data(self): + if self.lidar_is_outside_bb(): + print("Error!!! Points of lidar point cloud are outside its BB for car %d: %s " % (self._actor_id, self._actor_type)) + self.print() + return False + else: + return True + +def wait(world, frames=100, queue = None, slist = None): + for i in range(0, frames): + world.tick() + + if queue != None and slist != None: + try: + for _i in range (0, len(slist)): + s_frame = queue.get(True, 1.0) + except Empty: + print(" Some of the sensor information is missed") + +# Sensor callback. +# This is where you receive the sensor data and +# process it as you liked and the important part is that, +# at the end, it should include an element into the sensor queue. +def lidar_callback(sensor_data, sensor_queue, sensor_name): + sensor_pc_local = np.frombuffer(sensor_data.raw_data, dtype=np.dtype([ + ('x', np.float32), ('y', np.float32), ('z', np.float32), + ('CosAngle', np.float32), ('ObjIdx', np.uint32), ('ObjTag', np.uint32)])) + sensor_transf = sensor_data.transform + sensor_queue.put((sensor_data.frame, sensor_name, sensor_pc_local, sensor_transf)) + +def bb_callback(snapshot, world, sensor_queue, sensor_name): + data_array = [] + + vehicles = world.get_actors().filter('vehicle.*') + for actor in vehicles: + data_array.append((actor.id, actor.type_id, actor.get_transform(), actor.bounding_box)) + + sensor_queue.put((snapshot.frame, sensor_name, data_array)) + +def move_spectator(world, actor): + actor_tr = actor.get_transform() + spectator_transform = carla.Transform(actor_tr.location, actor_tr.rotation) + spectator_transform.location -= actor_tr.get_forward_vector() * 5 + spectator_transform.location -= actor_tr.get_up_vector() * 3 + spectator = world.get_spectator() + spectator.set_transform(spectator_transform) + +def world_callback(snapshot, world, sensor_queue, sensor_name, actor): + move_spectator(world, actor) + bb_callback(snapshot, world, sensor_queue, sensor_name) + +def process_sensors(w_frame, sensor_queue, sensor_number): + if sensor_number != 2: + print("Error!!! Sensor number should be two") + + sl_data = None + bb_data = None + + try: + for i in range (0, sensor_number): + s_frame = sensor_queue.get(True, 1.0) + while s_frame[0] != w_frame: + print("Warning! Missmatch for sensor %s in the frame timestamp (w: %d, s: %d)" % (s_frame[1], w_frame, s_frame[0])) + print("This could be due to accumulated data for previous steps") + s_frame = sensor_queue.get(True, 1.0) + + if s_frame[1] == "semlidar": + sl_data = s_frame + elif s_frame[1] == "bb": + bb_data = s_frame + #print(" Frame: %d Sensor: %s Len: %d " % (s_frame[0], s_frame[1], len(s_frame[2]))) + except Empty: + print("Error!!! The needeinformation is not here!!!") + return + + if sl_data == None or bb_data == None: + print("Error!!! Missmatch for sensor %s in the frame timestamp (w: %d, s: %d)" % (s_frame[1], w_frame, s_frame[0])) + + for actor_data in bb_data[2]: + trace_vehicle = ActorTrace(actor_data, sl_data) + trace_vehicle.process() + trace_vehicle.check_lidar_data() + +class SpawnCar(object): + def __init__(self, location, rotation, filter="vehicle.*", autopilot = False, velocity = None): + self._filter = filter + self._transform = carla.Transform(location, rotation) + self._autopilot = autopilot + self._velocity = velocity + self._actor = None + self._world = None + + def spawn(self, world): + self._world = world + actor_BP = world.get_blueprint_library().filter(self._filter)[0] + self._actor = world.spawn_actor(actor_BP, self._transform) + self._actor.set_autopilot(True) + + return self._actor + + def destroy(self): + if self._actor != None: + self._actor.destroy() + + +CarPropList = [ + SpawnCar(carla.Location(x=83, y= -40, z=5), carla.Rotation(yaw=-90), filter= "*lincoln*", autopilot=True), + SpawnCar(carla.Location(x=83, y= -30, z=3), carla.Rotation(yaw=-90), filter= "*ambulance*", autopilot=True), + SpawnCar(carla.Location(x=83, y= -20, z=3), carla.Rotation(yaw=-90), filter= "*etron*", autopilot=True), + SpawnCar(carla.Location(x=120, y= -3.5, z=2), carla.Rotation(yaw=+180), filter= "*isetta*", autopilot=True), + SpawnCar(carla.Location(x=100, y= -3.5, z=2), carla.Rotation(yaw=+180), filter= "*etron*", autopilot=True), + SpawnCar(carla.Location(x=140, y= -3.5, z=2), carla.Rotation(yaw=+180), filter= "*model3*", autopilot=True), + SpawnCar(carla.Location(x=160, y= -3.5, z=2), carla.Rotation(yaw=+180), filter= "*impala*", autopilot=False), + SpawnCar(carla.Location(x=180, y= -3.5, z=2), carla.Rotation(yaw=+180), filter= "*a2*", autopilot=True), + SpawnCar(carla.Location(x=60, y= +6, z=2), carla.Rotation(yaw=+00), filter= "*model3*", autopilot=True), + SpawnCar(carla.Location(x=80, y= +6, z=2), carla.Rotation(yaw=+00), filter= "*etron*", autopilot=True), + SpawnCar(carla.Location(x=100, y= +6, z=2), carla.Rotation(yaw=+00), filter= "*mustan*", autopilot=True), + SpawnCar(carla.Location(x=120, y= +6, z=2), carla.Rotation(yaw=+00), filter= "*isetta*", autopilot=True), + SpawnCar(carla.Location(x=140, y= +6, z=2), carla.Rotation(yaw=+00), filter= "*impala*", autopilot=True), + SpawnCar(carla.Location(x=160, y= +6, z=2), carla.Rotation(yaw=+00), filter= "*prius*", autopilot=True), + SpawnCar(carla.Location(x=234, y= +20,z=2), carla.Rotation(yaw=+90), filter= "*dodge*", autopilot=True), + SpawnCar(carla.Location(x=234, y= +40,z=2), carla.Rotation(yaw=+90), filter= "*isetta*", autopilot=True), + SpawnCar(carla.Location(x=234, y= +80,z=2), carla.Rotation(yaw=+90), filter= "*tt*", autopilot=True), + SpawnCar(carla.Location(x=243, y= -40,z=2), carla.Rotation(yaw=-90), filter= "*etron*", autopilot=True), + SpawnCar(carla.Location(x=243, y= -20,z=2), carla.Rotation(yaw=-90), filter= "*mkz2017*", autopilot=True), + SpawnCar(carla.Location(x=243, y= +00,z=2), carla.Rotation(yaw=-90), filter= "*mustan*", autopilot=True), + SpawnCar(carla.Location(x=243, y= +20,z=2), carla.Rotation(yaw=-90), filter= "*dodge*", autopilot=True), + SpawnCar(carla.Location(x=243, y= +40,z=2), carla.Rotation(yaw=-90), filter= "*charger2020*", autopilot=True), + SpawnCar(carla.Location(x=243, y= +60,z=2), carla.Rotation(yaw=-90), filter= "*lincoln2020*", autopilot=True), + SpawnCar(carla.Location(x=243, y= +80,z=2), carla.Rotation(yaw=-90), filter= "*tt*", autopilot=True), + SpawnCar(carla.Location(x=243, y=+100,z=2), carla.Rotation(yaw=-90), filter= "*a2*", autopilot=True), + SpawnCar(carla.Location(x=243, y=+120,z=2), carla.Rotation(yaw=-90), filter= "*wrangler_rubicon*", autopilot=True), + SpawnCar(carla.Location(x=243, y=+140,z=2), carla.Rotation(yaw=-90), filter= "*c3*", autopilot=True) +] + +def spawn_prop_vehicles(world): + for car in CarPropList: + car.spawn(world) + +def destroy_prop_vehicles(): + for car in CarPropList: + car.destroy() + + +def main(): + # We start creating the client + client = carla.Client('localhost', 2000) + client.set_timeout(2.0) + world = client.get_world() + + try: + # We need to save the settings to be able to recover them at the end + # of the script to leave the server in the same state that we found it. + original_settings = world.get_settings() + settings = world.get_settings() + + # We set CARLA syncronous mode + settings.fixed_delta_seconds = 0.05 + settings.synchronous_mode = True + world.apply_settings(settings) + + traffic_manager = client.get_trafficmanager(8000) + traffic_manager.set_synchronous_mode(True) + + # We create the sensor queue in which we keep track of the information + # already received. This structure is thread safe and can be + # accessed by all the sensors callback concurrently without problem. + sensor_queue = Queue() + + # Spawning ego vehicle + actor_BP = world.get_blueprint_library().filter("vehicle.lincoln.mkz2017")[0] + car_tr = carla.Transform(carla.Location(x=239, y=125, z=0.9), carla.Rotation(yaw=-88.5)) + actor = world.spawn_actor(actor_BP, car_tr) + + world.tick() + move_spectator(world, actor) + + spawn_prop_vehicles(world) + + wait(world, 10) + + # We create all the sensors and keep them in a list for convenience. + sensor_list = [] + + 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', '500000') + lidar_bp.set_attribute('range', '300') + lidar_bp.set_attribute('upper_fov', '10.0') + lidar_bp.set_attribute('lower_fov', '-90.0') + lidar_tr = carla.Transform(carla.Location(z=3), carla.Rotation(yaw=0)) + lidar = world.spawn_actor(lidar_bp, lidar_tr, attach_to=actor) + lidar.listen(lambda data: lidar_callback(data, sensor_queue, "semlidar")) + world.on_tick(lambda snapshot: world_callback(snapshot, world, sensor_queue, "bb", actor)) + sensor_list.append(lidar) + sensor_list.append(actor) # actor acts as a 'sensor' to simplify bb-lidar data comparison + + # Set autopilot for main vehicle + actor.enable_constant_velocity(carla.Vector3D(20, 0, 0)) + + for _i in range(0, 100): + # Tick the server + world.tick() + w_frame = world.get_snapshot().frame + process_sensors(w_frame, sensor_queue, len(sensor_list)) + + actor.disable_constant_velocity() + + finally: + world.apply_settings(original_settings) + + # Destroy all the actors + destroy_prop_vehicles() + for sensor in sensor_list: + sensor.destroy() + + +if __name__ == "__main__": + try: + main() + except KeyboardInterrupt: + print(' - Exited by user.') +