From 668303ef766597eb07e1530b8b2924ba7b8447aa Mon Sep 17 00:00:00 2001 From: Daniel Santos-Olivan Date: Mon, 26 Apr 2021 12:13:09 +0200 Subject: [PATCH] Added smoke to check the point count for Lidar and Semantic Lidar --- PythonAPI/test/smoke/test_lidar.py | 68 ++++++++++++++++++++++++++++-- 1 file changed, 65 insertions(+), 3 deletions(-) diff --git a/PythonAPI/test/smoke/test_lidar.py b/PythonAPI/test/smoke/test_lidar.py index 1b7255a5f..4191bdf16 100644 --- a/PythonAPI/test/smoke/test_lidar.py +++ b/PythonAPI/test/smoke/test_lidar.py @@ -12,17 +12,22 @@ import time import math import numpy as np from enum import Enum +from queue import Queue +from queue import Empty class SensorType(Enum): LIDAR = 1 SEMLIDAR = 2 class Sensor(): - def __init__(self, test, sensor_type, attributes): + def __init__(self, test, sensor_type, attributes, sensor_name = None, sensor_queue = None): self.test = test self.world = test.world self.sensor_type = sensor_type self.error = None + self.name = sensor_name + self.queue = sensor_queue + self.curr_det_pts = 0 if self.sensor_type == SensorType.LIDAR: self.bp_sensor = self.world.get_blueprint_library().filter("sensor.lidar.ray_cast")[0] @@ -35,13 +40,14 @@ class Sensor(): self.bp_sensor.set_attribute(key, attributes[key]) tranf = self.world.get_map().get_spawn_points()[0] + tranf.location.z += 3 self.sensor = self.world.spawn_actor(self.bp_sensor, tranf) - self.sensor.listen(lambda sensor_data: self.callback(sensor_data)) + self.sensor.listen(lambda sensor_data: self.callback(sensor_data, self.name, self.queue)) def destroy(self): self.sensor.destroy() - def callback(self, sensor_data): + def callback(self, sensor_data, sensor_name=None, queue=None): # Compute the total sum of points adding all channels total_channel_points = 0 for i in range(0, sensor_data.channels): @@ -57,12 +63,14 @@ class Sensor(): points = np.frombuffer(sensor_data.raw_data, dtype=np.dtype('f4')) points = np.reshape(points, (int(points.shape[0] / 4), 4)) total_np_points = points.shape[0] + self.curr_det_pts = total_np_points elif self.sensor_type == SensorType.SEMLIDAR: data = 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)])) points = np.array([data['x'], data['y'], data['z']]).T total_np_points = points.shape[0] + self.curr_det_pts = total_np_points else: self.error = "It should never reach this point" return @@ -73,9 +81,16 @@ class Sensor(): if total_channel_points != total_detect_points: self.error = "The sum of the points of all channels does not match with the LidarMeasurament array" + # Add option to synchronization queue + if queue is not None: + queue.put((sensor_data.frame, sensor_name, self.curr_det_pts)) + def is_correct(self): return self.error is None + def get_current_detection_points(): + return self.curr_det_pts + class TestSyncLidar(SyncSmokeTest): def test_lidar_point_count(self): print("TestSyncLidar.test_lidar_point_count") @@ -174,3 +189,50 @@ class TestASyncLidar(SmokeTest): for sensor in sensors: if not sensor.is_correct(): self.fail(sensor.error) + +class TestCompareLidars(SyncSmokeTest): + def test_lidar_comparison(self): + print("TestCompareLidars.test_lidar_comparison") + sensors = [] + + att_sem_lidar={'channels' : '64', 'range' : '200', 'points_per_second': '500000'} + att_lidar_nod={'channels' : '64', 'dropoff_intensity_limit': '0.0', 'dropoff_general_rate': '0.0', + 'range' : '200', 'points_per_second': '500000'} + att_lidar_def={'channels' : '64', 'range' : '200', 'points_per_second': '500000'} + + sensor_queue = Queue() + sensors.append(Sensor(self, SensorType.SEMLIDAR, att_sem_lidar, "SemLidar", sensor_queue)) + sensors.append(Sensor(self, SensorType.LIDAR, att_lidar_nod, "LidarNoD", sensor_queue)) + sensors.append(Sensor(self, SensorType.LIDAR, att_lidar_def, "LidarDef", sensor_queue)) + + for _ in range(0, 15): + self.world.tick() + + data_sem_lidar = None + data_lidar_nod = None + data_lidar_def = None + for _ in range(len(sensors)): + data = sensor_queue.get(True, 10.0) + if data[1] == "SemLidar": + data_sem_lidar = data + elif data[1] == "LidarNoD": + data_lidar_nod = data + elif data[1] == "LidarDef": + data_lidar_def = data + else: + self.fail("It should never reach this point") + + # Check that frame number are correct + self.assertEqual(data_sem_lidar[0], data_lidar_nod[0], "The frame numbers of LiDAR and SemLiDAR do not match.") + self.assertEqual(data_sem_lidar[0], data_lidar_def[0], "The frame numbers of LiDAR and SemLiDAR do not match.") + + # The detections of the semantic lidar and the Lidar with no dropoff should have the same point count always + self.assertEqual(data_sem_lidar[2], data_lidar_nod[2], "The point count of the detections of this frame of LiDAR(No dropoff) and SemLiDAR do not match.") + + # Default lidar should drop a minimum of 45% of the points so we check that but with a high tolerance to account for 'rare' cases + if data_lidar_def[2] > 0.75 * data_sem_lidar[2]: + self.fail("The point count of the default lidar should be much less than the Semantic Lidar point count.") + + time.sleep(1) + for sensor in sensors: + sensor.destroy()