372 lines
15 KiB
Python
372 lines
15 KiB
Python
|
#!/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>.
|
||
|
|
|
||
|
|
"""
|
||
|
|
|
||
|
|
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.')
|
||
|
|
|