diff --git a/README.md b/README.md index 0432015e..456ab343 100644 --- a/README.md +++ b/README.md @@ -84,4 +84,4 @@ The robot controller (brain folder) is the main controller of the ego vehicle. Behavior Metrics uses a publish/subscribe design to communicate with the simulator -![alt text](./assets/behavior_metrics_publish_subscribe.png) \ No newline at end of file +![alt text](./assets/behavior_metrics_publish_subscribe.png) diff --git a/assets/behavior_metrics_full_architecture.png b/assets/behavior_metrics_full_architecture.png deleted file mode 100644 index d69552c0..00000000 Binary files a/assets/behavior_metrics_full_architecture.png and /dev/null differ diff --git a/behavior_metrics/brains/CARLA/brain_carla_subjective_vision_deep_learning_previous_v.py b/behavior_metrics/brains/CARLA/brain_carla_subjective_vision_deep_learning_previous_v.py index 0185c3f0..7b967041 100644 --- a/behavior_metrics/brains/CARLA/brain_carla_subjective_vision_deep_learning_previous_v.py +++ b/behavior_metrics/brains/CARLA/brain_carla_subjective_vision_deep_learning_previous_v.py @@ -77,7 +77,10 @@ def __init__(self, sensors, actuators, handler, model, config=None): self.world.unload_map_layer(carla.MapLayer.Particles) time.sleep(5) - self.vehicle = self.world.get_actors().filter('vehicle.*')[0] + self.vehicle = next( + (p for p in self.world.get_actors() if 'vehicle' in p.type_id and p.attributes.get('role_name') == 'ego_vehicle'), + None + ) if model: if not path.exists(PRETRAINED_MODELS + model): @@ -124,7 +127,7 @@ def execute(self): image_2 = self.camera_2.getImage().data image_3 = self.camera_3.getImage().data - cropped = image[230:-1,:] + cropped = image[200:-1,:] if self.cont < 20: self.cont += 1 @@ -195,6 +198,10 @@ def execute(self): self.motors.sendThrottle(throttle) self.motors.sendSteer(steer) self.motors.sendBrake(break_command) + + if vehicle_speed >= 35: + self.motors.sendThrottle(0.0) + self.motors.sendBrake(0.0) if self.previous_commanded_throttle != None: a = np.array((throttle, steer, break_command)) diff --git a/behavior_metrics/carla-birdeye-view b/behavior_metrics/carla-birdeye-view new file mode 160000 index 00000000..9f4255cd --- /dev/null +++ b/behavior_metrics/carla-birdeye-view @@ -0,0 +1 @@ +Subproject commit 9f4255cd96c8241d5849ea2bcb52b8b3e5504e52 diff --git a/behavior_metrics/configs/CARLA/CARLA_launch_files/CARLA_object_files/single_ad_car_subjective_vision.json b/behavior_metrics/configs/CARLA/CARLA_launch_files/CARLA_object_files/single_ad_car_subjective_vision.json new file mode 100644 index 00000000..0a9d8fce --- /dev/null +++ b/behavior_metrics/configs/CARLA/CARLA_launch_files/CARLA_object_files/single_ad_car_subjective_vision.json @@ -0,0 +1,177 @@ +{ + "objects": + [ + { + "type": "sensor.pseudo.traffic_lights", + "id": "traffic_lights" + }, + { + "type": "sensor.pseudo.objects", + "id": "objects" + }, + { + "type": "sensor.pseudo.actor_list", + "id": "actor_list" + }, + { + "type": "sensor.pseudo.markers", + "id": "markers" + }, + { + "type": "sensor.pseudo.opendrive_map", + "id": "map" + }, + { + "type": "vehicle.tesla.model3", + "id": "ego_vehicle", + "sensors": + [ + { + "type": "sensor.camera.rgb", + "id": "rgb_front", + "spawn_point": {"x": 2.5, "y": 0.0, "z": 0.8, "roll": 0.0, "pitch": 0.0, "yaw": 0.0}, + "image_size_x": 640, + "image_size_y": 480 + }, + { + "type": "sensor.camera.rgb", + "id": "rgb_view", + "spawn_point": {"x": -4.5, "y": 0.0, "z": 2.8, "roll": 0.0, "pitch": 20.0, "yaw": 0.0}, + "image_size_x": 800, + "image_size_y": 600, + "fov": 90.0, + "attached_objects": + [ + { + "type": "actor.pseudo.control", + "id": "control" + } + ] + }, + { + "type": "sensor.lidar.ray_cast", + "id": "lidar", + "spawn_point": {"x": 0.0, "y": 0.0, "z": 2.4, "roll": 0.0, "pitch": 0.0, "yaw": 0.0}, + "range": 50, + "channels": 32, + "points_per_second": 320000, + "upper_fov": 2.0, + "lower_fov": -26.8, + "rotation_frequency": 20, + "noise_stddev": 0.0 + }, + { + "type": "sensor.lidar.ray_cast_semantic", + "id": "semantic_lidar", + "spawn_point": {"x": 0.0, "y": 0.0, "z": 2.4, "roll": 0.0, "pitch": 0.0, "yaw": 0.0}, + "range": 50, + "channels": 32, + "points_per_second": 320000, + "upper_fov": 2.0, + "lower_fov": -26.8, + "rotation_frequency": 20 + }, + { + "type": "sensor.other.radar", + "id": "radar_front", + "spawn_point": {"x": 2.0, "y": 0.0, "z": 2.0, "roll": 0.0, "pitch": 0.0, "yaw": 0.0}, + "horizontal_fov": 30.0, + "vertical_fov": 10.0, + "points_per_second": 1500, + "range": 100.0 + }, + { + "type": "sensor.camera.semantic_segmentation", + "id": "semantic_segmentation_front", + "spawn_point": {"x": 2.0, "y": 0.0, "z": 2.0, "roll": 0.0, "pitch": 0.0, "yaw": 0.0}, + "fov": 90.0, + "image_size_x": 400, + "image_size_y": 70 + }, + { + "type": "sensor.camera.depth", + "id": "depth_front", + "spawn_point": {"x": 2.0, "y": 0.0, "z": 2.0, "roll": 0.0, "pitch": 0.0, "yaw": 0.0}, + "fov": 90.0, + "image_size_x": 400, + "image_size_y": 70 + }, + { + "type": "sensor.camera.dvs", + "id": "dvs_front", + "spawn_point": {"x": 2.0, "y": 0.0, "z": 2.0, "roll": 0.0, "pitch": 0.0, "yaw": 0.0}, + "fov": 90.0, + "image_size_x": 400, + "image_size_y": 70, + "positive_threshold": 0.3, + "negative_threshold": 0.3, + "sigma_positive_threshold": 0.0, + "sigma_negative_threshold": 0.0, + "use_log": true, + "log_eps": 0.001 + }, + { + "type": "sensor.other.gnss", + "id": "gnss", + "spawn_point": {"x": 1.0, "y": 0.0, "z": 2.0}, + "noise_alt_stddev": 0.0, "noise_lat_stddev": 0.0, "noise_lon_stddev": 0.0, + "noise_alt_bias": 0.0, "noise_lat_bias": 0.0, "noise_lon_bias": 0.0 + }, + { + "type": "sensor.other.imu", + "id": "imu", + "spawn_point": {"x": 2.0, "y": 0.0, "z": 2.0, "roll": 0.0, "pitch": 0.0, "yaw": 0.0}, + "noise_accel_stddev_x": 0.0, "noise_accel_stddev_y": 0.0, "noise_accel_stddev_z": 0.0, + "noise_gyro_stddev_x": 0.0, "noise_gyro_stddev_y": 0.0, "noise_gyro_stddev_z": 0.0, + "noise_gyro_bias_x": 0.0, "noise_gyro_bias_y": 0.0, "noise_gyro_bias_z": 0.0 + }, + { + "type": "sensor.other.collision", + "id": "collision", + "spawn_point": {"x": 0.0, "y": 0.0, "z": 0.0} + }, + { + "type": "sensor.other.lane_invasion", + "id": "lane_invasion", + "spawn_point": {"x": 0.0, "y": 0.0, "z": 0.0} + }, + { + "type": "sensor.pseudo.tf", + "id": "tf" + }, + { + "type": "sensor.pseudo.objects", + "id": "objects" + }, + { + "type": "sensor.pseudo.odom", + "id": "odometry" + }, + { + "type": "sensor.pseudo.speedometer", + "id": "speedometer" + }, + { + "type": "actor.pseudo.control", + "id": "control" + } + ] + }, + { + "type": "vehicle.audi.a2", + "id": "npc_vehicle_1", + "spawn_point": {"x": 60.0, "y": 2.0, "z": 1.37, "roll": 0.0, "pitch": 0.0, "yaw": 180.0}, + "sensors": + [ + { + "type": "sensor.pseudo.objects", + "id": "objects" + }, + { + "type": "sensor.pseudo.odom", + "id": "odometry" + } + ] + } + ] +} diff --git a/behavior_metrics/configs/CARLA/CARLA_launch_files/CARLA_object_files/single_ad_car_town02.json b/behavior_metrics/configs/CARLA/CARLA_launch_files/CARLA_object_files/single_ad_car_town02.json new file mode 100644 index 00000000..3438109a --- /dev/null +++ b/behavior_metrics/configs/CARLA/CARLA_launch_files/CARLA_object_files/single_ad_car_town02.json @@ -0,0 +1,178 @@ +{ + "objects": + [ + { + "type": "sensor.pseudo.traffic_lights", + "id": "traffic_lights" + }, + { + "type": "sensor.pseudo.objects", + "id": "objects" + }, + { + "type": "sensor.pseudo.actor_list", + "id": "actor_list" + }, + { + "type": "sensor.pseudo.markers", + "id": "markers" + }, + { + "type": "sensor.pseudo.opendrive_map", + "id": "map" + }, + { + "type": "vehicle.tesla.model3", + "id": "ego_vehicle", + "sensors": + [ + { + "type": "sensor.camera.rgb", + "id": "rgb_front", + "spawn_point": {"x": 2.0, "y": 0.0, "z": 2.0, "roll": 0.0, "pitch": 0.0, "yaw": 0.0}, + "image_size_x": 800, + "image_size_y": 600, + "fov": 90.0 + }, + { + "type": "sensor.camera.rgb", + "id": "rgb_view", + "spawn_point": {"x": -4.5, "y": 0.0, "z": 2.8, "roll": 0.0, "pitch": 20.0, "yaw": 0.0}, + "image_size_x": 800, + "image_size_y": 600, + "fov": 90.0, + "attached_objects": + [ + { + "type": "actor.pseudo.control", + "id": "control" + } + ] + }, + { + "type": "sensor.lidar.ray_cast", + "id": "lidar", + "spawn_point": {"x": 0.0, "y": 0.0, "z": 2.4, "roll": 0.0, "pitch": 0.0, "yaw": 0.0}, + "range": 50, + "channels": 32, + "points_per_second": 320000, + "upper_fov": 2.0, + "lower_fov": -26.8, + "rotation_frequency": 20, + "noise_stddev": 0.0 + }, + { + "type": "sensor.lidar.ray_cast_semantic", + "id": "semantic_lidar", + "spawn_point": {"x": 0.0, "y": 0.0, "z": 2.4, "roll": 0.0, "pitch": 0.0, "yaw": 0.0}, + "range": 50, + "channels": 32, + "points_per_second": 320000, + "upper_fov": 2.0, + "lower_fov": -26.8, + "rotation_frequency": 20 + }, + { + "type": "sensor.other.radar", + "id": "radar_front", + "spawn_point": {"x": 2.0, "y": 0.0, "z": 2.0, "roll": 0.0, "pitch": 0.0, "yaw": 0.0}, + "horizontal_fov": 30.0, + "vertical_fov": 10.0, + "points_per_second": 1500, + "range": 100.0 + }, + { + "type": "sensor.camera.semantic_segmentation", + "id": "semantic_segmentation_front", + "spawn_point": {"x": 2.0, "y": 0.0, "z": 2.0, "roll": 0.0, "pitch": 0.0, "yaw": 0.0}, + "fov": 90.0, + "image_size_x": 400, + "image_size_y": 70 + }, + { + "type": "sensor.camera.depth", + "id": "depth_front", + "spawn_point": {"x": 2.0, "y": 0.0, "z": 2.0, "roll": 0.0, "pitch": 0.0, "yaw": 0.0}, + "fov": 90.0, + "image_size_x": 400, + "image_size_y": 70 + }, + { + "type": "sensor.camera.dvs", + "id": "dvs_front", + "spawn_point": {"x": 2.0, "y": 0.0, "z": 2.0, "roll": 0.0, "pitch": 0.0, "yaw": 0.0}, + "fov": 90.0, + "image_size_x": 400, + "image_size_y": 70, + "positive_threshold": 0.3, + "negative_threshold": 0.3, + "sigma_positive_threshold": 0.0, + "sigma_negative_threshold": 0.0, + "use_log": true, + "log_eps": 0.001 + }, + { + "type": "sensor.other.gnss", + "id": "gnss", + "spawn_point": {"x": 1.0, "y": 0.0, "z": 2.0}, + "noise_alt_stddev": 0.0, "noise_lat_stddev": 0.0, "noise_lon_stddev": 0.0, + "noise_alt_bias": 0.0, "noise_lat_bias": 0.0, "noise_lon_bias": 0.0 + }, + { + "type": "sensor.other.imu", + "id": "imu", + "spawn_point": {"x": 2.0, "y": 0.0, "z": 2.0, "roll": 0.0, "pitch": 0.0, "yaw": 0.0}, + "noise_accel_stddev_x": 0.0, "noise_accel_stddev_y": 0.0, "noise_accel_stddev_z": 0.0, + "noise_gyro_stddev_x": 0.0, "noise_gyro_stddev_y": 0.0, "noise_gyro_stddev_z": 0.0, + "noise_gyro_bias_x": 0.0, "noise_gyro_bias_y": 0.0, "noise_gyro_bias_z": 0.0 + }, + { + "type": "sensor.other.collision", + "id": "collision", + "spawn_point": {"x": 0.0, "y": 0.0, "z": 0.0} + }, + { + "type": "sensor.other.lane_invasion", + "id": "lane_invasion", + "spawn_point": {"x": 0.0, "y": 0.0, "z": 0.0} + }, + { + "type": "sensor.pseudo.tf", + "id": "tf" + }, + { + "type": "sensor.pseudo.objects", + "id": "objects" + }, + { + "type": "sensor.pseudo.odom", + "id": "odometry" + }, + { + "type": "sensor.pseudo.speedometer", + "id": "speedometer" + }, + { + "type": "actor.pseudo.control", + "id": "control" + } + ] + }, + { + "type": "vehicle.audi.a2", + "id": "npc_vehicle_1", + "spawn_point": {"x": 194.0, "y": -250, "z": 1.37, "roll": 0.0, "pitch": 0.0, "yaw": 90.0}, + "sensors": + [ + { + "type": "sensor.pseudo.objects", + "id": "objects" + }, + { + "type": "sensor.pseudo.odom", + "id": "odometry" + } + ] + } + ] +} \ No newline at end of file diff --git a/behavior_metrics/configs/CARLA/CARLA_launch_files/town_01_anticlockwise.launch b/behavior_metrics/configs/CARLA/CARLA_launch_files/town_01_anticlockwise.launch index 45db5b41..01ca1e1e 100644 --- a/behavior_metrics/configs/CARLA/CARLA_launch_files/town_01_anticlockwise.launch +++ b/behavior_metrics/configs/CARLA/CARLA_launch_files/town_01_anticlockwise.launch @@ -56,7 +56,7 @@ - + diff --git a/behavior_metrics/configs/CARLA/CARLA_launch_files/town_01_clockwise.launch b/behavior_metrics/configs/CARLA/CARLA_launch_files/town_01_clockwise.launch index 7375d454..b4656122 100644 --- a/behavior_metrics/configs/CARLA/CARLA_launch_files/town_01_clockwise.launch +++ b/behavior_metrics/configs/CARLA/CARLA_launch_files/town_01_clockwise.launch @@ -28,7 +28,7 @@ - + diff --git a/behavior_metrics/configs/CARLA/CARLA_launch_files/town_02_anticlockwise.launch b/behavior_metrics/configs/CARLA/CARLA_launch_files/town_02_anticlockwise.launch index ef688bd2..e56b89bb 100644 --- a/behavior_metrics/configs/CARLA/CARLA_launch_files/town_02_anticlockwise.launch +++ b/behavior_metrics/configs/CARLA/CARLA_launch_files/town_02_anticlockwise.launch @@ -55,7 +55,7 @@ - + diff --git a/behavior_metrics/configs/CARLA/CARLA_launch_files/town_02_anticlockwise_low.launch b/behavior_metrics/configs/CARLA/CARLA_launch_files/town_02_anticlockwise_low.launch deleted file mode 100644 index 30b69148..00000000 --- a/behavior_metrics/configs/CARLA/CARLA_launch_files/town_02_anticlockwise_low.launch +++ /dev/null @@ -1,80 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/behavior_metrics/configs/CARLA/CARLA_launch_files/town_02_anticlockwise_single_ad_low.launch b/behavior_metrics/configs/CARLA/CARLA_launch_files/town_02_anticlockwise_single_ad_low.launch new file mode 100644 index 00000000..c0822290 --- /dev/null +++ b/behavior_metrics/configs/CARLA/CARLA_launch_files/town_02_anticlockwise_single_ad_low.launch @@ -0,0 +1,53 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behavior_metrics/configs/CARLA/CARLA_launch_files/town_02_anticlockwise_single_ad_npc_low.launch b/behavior_metrics/configs/CARLA/CARLA_launch_files/town_02_anticlockwise_single_ad_npc_low.launch new file mode 100644 index 00000000..2a29d3ab --- /dev/null +++ b/behavior_metrics/configs/CARLA/CARLA_launch_files/town_02_anticlockwise_single_ad_npc_low.launch @@ -0,0 +1,71 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behavior_metrics/configs/CARLA/CARLA_launch_files/town_02_clockwise.launch b/behavior_metrics/configs/CARLA/CARLA_launch_files/town_02_clockwise.launch index f3f963c4..87111085 100644 --- a/behavior_metrics/configs/CARLA/CARLA_launch_files/town_02_clockwise.launch +++ b/behavior_metrics/configs/CARLA/CARLA_launch_files/town_02_clockwise.launch @@ -26,7 +26,7 @@ - + diff --git a/behavior_metrics/configs/CARLA/default_carla_subjective_vision.yml b/behavior_metrics/configs/CARLA/default_carla_subjective_vision.yml index fc0be152..e9df8537 100644 --- a/behavior_metrics/configs/CARLA/default_carla_subjective_vision.yml +++ b/behavior_metrics/configs/CARLA/default_carla_subjective_vision.yml @@ -37,7 +37,7 @@ Behaviors: PilotTimeCycle: 100 AsyncMode: False Parameters: - Model: '20230428-103315_pilotnet_model_3_151_cp.h5' + Model: 'pilotnet_starstar_traffic_6.h5' ImageCropped: True ImageSize: [ 200,66 ] ImageNormalized: True @@ -47,8 +47,10 @@ Behaviors: ImageTranform: '' Type: 'CARLA' Simulation: - World: configs/CARLA/CARLA_launch_files/town_02_anticlockwise_low.launch + World: configs/CARLA/CARLA_launch_files/town_02_anticlockwise_single_ad_low.launch RandomSpawnPoint: False + MultiCar: True + NumberOfVehicle: 0 Dataset: In: '/tmp/my_bag.bag' Out: '' diff --git a/behavior_metrics/driver_carla.py b/behavior_metrics/driver_carla.py index 088cd59e..94a721d3 100644 --- a/behavior_metrics/driver_carla.py +++ b/behavior_metrics/driver_carla.py @@ -250,7 +250,23 @@ def generate_agregated_experiments_metrics(experiments_starting_time, experiment }, { 'metric': 'suddenness_distance_speed_per_km', - 'title': 'uddenness distance speed per km per experiment' + 'title': 'Suddenness distance speed per km per experiment' + }, + { + 'metric': 'dangerous_distance_pct_km', + 'title': 'Percentage of dangerous distance per km' + }, + { + 'metric': 'close_distance_pct_km', + 'title': 'Percentage of close distance per km' + }, + { + 'metric': 'medium_distance_pct_km', + 'title': 'Percentage of medium distance per km' + }, + { + 'metric': 'great_distance_pct_km', + 'title': 'Percentage of great distance per km' }, { 'metric': 'completed_laps', diff --git a/behavior_metrics/ui/gui/views/stats_window.py b/behavior_metrics/ui/gui/views/stats_window.py index 9155cbec..1c6ad940 100644 --- a/behavior_metrics/ui/gui/views/stats_window.py +++ b/behavior_metrics/ui/gui/views/stats_window.py @@ -102,10 +102,10 @@ def __init__(self, parent=None, controller=None): self.layout.addWidget(self.mean_brain_iterations_simulated_time_label) self.brain_iterations_frequency_simulated_time_label = QLabel("Brain iterations frequency simulated time-> " + str(self.controller.experiment_metrics['brain_iterations_frequency_simulated_time']) + ' it/s') self.layout.addWidget(self.brain_iterations_frequency_simulated_time_label) - self.mean_inference_time_label = QLabel("GPU mean inference time -> " + str(self.controller.experiment_metrics['gpu_mean_inference_time']) + ' s') - self.layout.addWidget(self.mean_inference_time_label) - self.mean_inference_time_label = QLabel("GPU inference frequency -> " + str(self.controller.experiment_metrics['gpu_inference_frequency']) + ' it/s') - self.layout.addWidget(self.mean_inference_time_label) + self.gpu_mean_inference_time_label = QLabel("GPU mean inference time -> " + str(self.controller.experiment_metrics['gpu_mean_inference_time']) + ' s') + self.layout.addWidget(self.gpu_mean_inference_time_label) + self.gpu_inference_frequency_label = QLabel("GPU inference frequency -> " + str(self.controller.experiment_metrics['gpu_inference_frequency']) + ' it/s') + self.layout.addWidget(self.gpu_inference_frequency_label) self.gpu_inference_label = QLabel("GPU inference -> " + str(self.controller.experiment_metrics['gpu_inference'])) self.layout.addWidget(self.gpu_inference_label) self.suddenness_distance_control_commands_label = QLabel("Suddenness distance control commands -> " + str(self.controller.experiment_metrics['suddenness_distance_control_commands'])) @@ -128,5 +128,14 @@ def __init__(self, parent=None, controller=None): self.layout.addWidget(self.suddenness_distance_speed_label) self.suddenness_distance_speed_per_km_label = QLabel("Suddenness distance speed per km -> " + str(self.controller.experiment_metrics['suddenness_distance_speed_per_km'])) self.layout.addWidget(self.suddenness_distance_speed_per_km_label) + if 'dangerous_distance_pct_km' in self.controller.experiment_metrics: + self.dangerous_distance_pct_km = QLabel("Percentage of dangerous distance per km -> " + str(self.controller.experiment_metrics['dangerous_distance_pct_km'])) + self.layout.addWidget(self.dangerous_distance_pct_km) + self.close_distance_pct_km = QLabel("Percentage of close distance per km -> " + str(self.controller.experiment_metrics['close_distance_pct_km'])) + self.layout.addWidget(self.close_distance_pct_km) + self.medium_distance_pct_km = QLabel("Percentage of medium distance per km -> " + str(self.controller.experiment_metrics['medium_distance_pct_km'])) + self.layout.addWidget(self.medium_distance_pct_km) + self.great_distance_pct_km = QLabel("Percentage of great distance per km -> " + str(self.controller.experiment_metrics['great_distance_pct_km'])) + self.layout.addWidget(self.great_distance_pct_km) wid.setLayout(self.layout) \ No newline at end of file diff --git a/behavior_metrics/utils/configuration.py b/behavior_metrics/utils/configuration.py index 02adf75a..156d2376 100644 --- a/behavior_metrics/utils/configuration.py +++ b/behavior_metrics/utils/configuration.py @@ -141,6 +141,11 @@ def initialize_configuration(self, config_data): else: self.spawn_points = [] + if 'MultiCar' in config_data['Behaviors']['Simulation']: + self.multicar = config_data['Behaviors']['Simulation']['MultiCar'] + else: + self.multicar = False + if 'NumberOfVehicle' in config_data['Behaviors']['Simulation']: self.number_of_vehicle = config_data['Behaviors']['Simulation']['NumberOfVehicle'] else: diff --git a/behavior_metrics/utils/controller_carla.py b/behavior_metrics/utils/controller_carla.py index 81742325..5290a7ff 100644 --- a/behavior_metrics/utils/controller_carla.py +++ b/behavior_metrics/utils/controller_carla.py @@ -266,23 +266,23 @@ def record_metrics(self, metrics_record_dir_path, world_counter=None, brain_coun self.experiment_metrics['experiment_model'] = self.pilot.configuration.experiment_model[brain_counter] else: self.experiment_metrics['experiment_model'] = self.pilot.configuration.experiment_model + if hasattr(self.pilot.configuration, 'experiment_name'): self.experiment_metrics['experiment_name'] = self.pilot.configuration.experiment_name - if brain_counter is not None: - self.experiment_metrics['experiment_model'] = self.pilot.configuration.experiment_model[brain_counter] - else: - self.experiment_metrics['experiment_model'] = self.pilot.configuration.experiment_model + if hasattr(self.pilot.configuration, 'experiment_name'): self.experiment_metrics['experiment_name'] = self.pilot.configuration.experiment_name self.experiment_metrics['experiment_description'] = self.pilot.configuration.experiment_description self.experiment_metrics['experiment_timeout'] = self.pilot.configuration.experiment_timeouts[world_counter] self.experiment_metrics['experiment_repetition'] = repetition_counter + self.metrics_record_dir_path = metrics_record_dir_path os.mkdir(self.metrics_record_dir_path + self.time_str) self.experiment_metrics_bag_filename = self.metrics_record_dir_path + self.time_str + '/' + self.time_str + '.bag' topics = [ + '/carla/npc_vehicle_1/odometry', '/carla/ego_vehicle/odometry', '/carla/ego_vehicle/collision', '/carla/ego_vehicle/lane_invasion', @@ -329,13 +329,21 @@ def stop_recording_metrics(self): self.experiment_metrics['gpu_mean_inference_time'] = sum(self.pilot.brains.active_brain.inference_times) / len(self.pilot.brains.active_brain.inference_times) self.experiment_metrics['gpu_inference_frequency'] = 1 / self.experiment_metrics['gpu_mean_inference_time'] self.experiment_metrics['gpu_inference'] = self.pilot.brains.active_brain.gpu_inference + else: + self.experiment_metrics['gpu_mean_inference_time'] = 0 + self.experiment_metrics['gpu_inference_frequency'] = 0 + self.experiment_metrics['gpu_inference'] = 0 if hasattr(self.pilot.brains.active_brain, 'bird_eye_view_images'): - self.experiment_metrics['brain_iterations_simulated_time'] = len(self.pilot.brain_iterations_simulated_time) self.experiment_metrics['bird_eye_view_images'] = self.pilot.brains.active_brain.bird_eye_view_images self.experiment_metrics['bird_eye_view_unique_images'] = self.pilot.brains.active_brain.bird_eye_view_unique_images self.experiment_metrics['bird_eye_view_unique_images_percentage'] = self.experiment_metrics['bird_eye_view_unique_images'] / self.experiment_metrics['bird_eye_view_images'] + else: + self.experiment_metrics['bird_eye_view_images'] = 0 + self.experiment_metrics['bird_eye_view_unique_images'] = 0 + self.experiment_metrics['bird_eye_view_unique_images_percentage'] = 0 + self.experiment_metrics['brain_iterations_simulated_time'] = len(self.pilot.brain_iterations_simulated_time) self.experiment_metrics['mean_brain_iterations_real_time'] = mean_brain_iterations_real_time self.experiment_metrics['brain_iterations_frequency_real_time'] = brain_iterations_frequency_real_time self.experiment_metrics['target_brain_iterations_real_time'] = target_brain_iterations_real_time @@ -344,7 +352,7 @@ def stop_recording_metrics(self): self.experiment_metrics['experiment_total_real_time'] = end_time - self.pilot.pilot_start_time experiment_metrics_filename = self.metrics_record_dir_path + self.time_str + '/' + self.time_str - self.experiment_metrics = metrics_carla.get_metrics(self.experiment_metrics, self.experiment_metrics_bag_filename, self.map_waypoints, experiment_metrics_filename) + self.experiment_metrics = metrics_carla.get_metrics(self.experiment_metrics, self.experiment_metrics_bag_filename, self.map_waypoints, experiment_metrics_filename, self.pilot.configuration) self.save_metrics(first_images, last_images) for key, value in self.experiment_metrics.items(): diff --git a/behavior_metrics/utils/metrics_carla.py b/behavior_metrics/utils/metrics_carla.py index 549f02bc..3fa87434 100644 --- a/behavior_metrics/utils/metrics_carla.py +++ b/behavior_metrics/utils/metrics_carla.py @@ -46,7 +46,8 @@ def circuit_distance_completed(checkpoints, lap_point): return diameter -def get_metrics(experiment_metrics, experiment_metrics_bag_filename, map_waypoints, experiment_metrics_filename): +def get_metrics(experiment_metrics, experiment_metrics_bag_filename, map_waypoints, experiment_metrics_filename, config): + time_counter = 5 while not os.path.exists(experiment_metrics_bag_filename): time.sleep(1) @@ -71,6 +72,13 @@ def get_metrics(experiment_metrics, experiment_metrics_bag_filename, map_waypoin for index, row in dataframe_pose.iterrows(): checkpoints.append(row) + if config.multicar: + data_file = experiment_metrics_bag_filename.split('.bag')[0] + '/carla-npc_vehicle_1-odometry.csv' + dataframe_pose = pd.read_csv(data_file) + checkpoints_2 = [] + for index, row in dataframe_pose.iterrows(): + checkpoints_2.append(row) + data_file = experiment_metrics_bag_filename.split('.bag')[0] + '/clock.csv' dataframe_clock = pd.read_csv(data_file) clock_points = [] @@ -116,6 +124,8 @@ def get_metrics(experiment_metrics, experiment_metrics_bag_filename, map_waypoin experiment_metrics, collisions_checkpoints = get_collisions(experiment_metrics, collision_points, dataframe_pose) experiment_metrics, lane_invasion_checkpoints = get_lane_invasions(experiment_metrics, lane_invasion_points, dataframe_pose) experiment_metrics['experiment_total_simulated_time'] = seconds_end - seconds_start + if config.multicar: + experiment_metrics = get_distance_other_vehicle(experiment_metrics, checkpoints, checkpoints_2) if 'bird_eye_view_images' in experiment_metrics: experiment_metrics['bird_eye_view_images_per_second'] = experiment_metrics['bird_eye_view_images'] / experiment_metrics['experiment_total_simulated_time'] @@ -630,3 +640,53 @@ def get_all_experiments_aggregated_metrics_boxplot(result, experiments_starting_ plt.ylim(0, max_value+max_value*0.1) plt.savefig(experiments_starting_time_str + '/' + experiment_metric_and_title['metric'] + '_boxplot.png', bbox_inches='tight') plt.close() + +def get_distance_other_vehicle(experiment_metrics, checkpoints, checkpoints_2): + dangerous_distance = 0 + close_distance = 0 + medium_distance = 0 + great_distance = 0 + total_distance = 0 + + dangerous_distance_pct_km = 0 + close_distance_pct_km = 0 + medium_distance_pct_km = 0 + great_distance_pct_km = 0 + total_distance_pct_km = 0 + + for i, (point, point_2) in enumerate(zip(checkpoints, checkpoints_2)): + current_checkpoint = np.array([point['pose.pose.position.x'], point['pose.pose.position.y']]) + current_checkpoint_2 = np.array([point_2['pose.pose.position.x'], point_2['pose.pose.position.y']]) + + if i != 0: + distance_front = np.linalg.norm(current_checkpoint - current_checkpoint_2) + distance = np.linalg.norm(previous_point - current_checkpoint) + + # Analyzing + if 20 < distance_front < 50: + great_distance += distance + total_distance += distance + elif 15 < distance_front <= 20: + medium_distance += distance + total_distance += distance + elif 6 < distance_front <= 15: + close_distance += distance + total_distance += distance + elif distance_front <= 6: + dangerous_distance += distance + total_distance += distance + + previous_point = current_checkpoint + + experiment_metrics['dangerous_distance_km'] = dangerous_distance + experiment_metrics['close_distance_km'] = close_distance + experiment_metrics['medium_distance_km'] = medium_distance + experiment_metrics['great_distance_km'] = great_distance + experiment_metrics['total_distance_to_front_car'] = total_distance + + experiment_metrics['dangerous_distance_pct_km'] = (total_distance and dangerous_distance / total_distance or 0) * 100 + experiment_metrics['close_distance_pct_km'] = (total_distance and close_distance / total_distance or 0) * 100 + experiment_metrics['medium_distance_pct_km'] = (total_distance and medium_distance / total_distance or 0) * 100 + experiment_metrics['great_distance_pct_km'] = (total_distance and great_distance / total_distance or 0) * 100 + + return experiment_metrics \ No newline at end of file diff --git a/behavior_metrics/utils/traffic.py b/behavior_metrics/utils/traffic.py index e910f19f..09d9b6bf 100644 --- a/behavior_metrics/utils/traffic.py +++ b/behavior_metrics/utils/traffic.py @@ -34,6 +34,11 @@ def __init__(self, n_vehicle, n_walker, percentage_walker_running=0.0, percentag self.walker_ids = [] def generate_traffic(self): + for p in self.world.get_actors(): + if 'vehicle' in p.type_id: + if p.attributes['role_name'] != 'ego_vehicle': + p.set_autopilot(True, self.traffic_manager.get_port()) + if self.n_vehicle > 0: self.spawn_vehicles(self.world, self.client, self.n_vehicle, self.traffic_manager) if self.n_walker > 0: @@ -107,6 +112,7 @@ def spawn_vehicles(self, world, client, n_vehicles, traffic_manager): blueprint.set_attribute('driver_id', driver_id) blueprint.set_attribute('role_name', 'autopilot') + batch.append(spawn_actor(blueprint, transform) .then(set_autopilot(future_actor, True, traffic_manager.get_port())))