diff --git a/behavior_metrics/brains/CARLA/brain_carla_autopilot.py b/behavior_metrics/brains/CARLA/brain_carla_autopilot.py new file mode 100644 index 00000000..bf2fd5a8 --- /dev/null +++ b/behavior_metrics/brains/CARLA/brain_carla_autopilot.py @@ -0,0 +1,94 @@ +#!/usr/bin/python +# -*- coding: utf-8 -*- +import csv +import cv2 +import math +import numpy as np +import threading +import time +import carla +from albumentations import ( + Compose, Normalize, RandomRain, RandomBrightness, RandomShadow, RandomSnow, RandomFog, RandomSunFlare +) +from utils.constants import DATASETS_DIR, ROOT_PATH + + + +GENERATED_DATASETS_DIR = ROOT_PATH + '/' + DATASETS_DIR + + +class Brain: + + def __init__(self, sensors, actuators, handler, config=None): + self.camera = sensors.get_camera('camera_0') + self.camera_1 = sensors.get_camera('camera_1') + self.camera_2 = sensors.get_camera('camera_2') + self.camera_3 = sensors.get_camera('camera_3') + + self.pose = sensors.get_pose3d('pose3d_0') + + self.bird_eye_view = sensors.get_bird_eye_view('bird_eye_view_0') + + self.motors = actuators.get_motor('motors_0') + self.handler = handler + self.config = config + + self.threshold_image = np.zeros((640, 360, 3), np.uint8) + self.color_image = np.zeros((640, 360, 3), np.uint8) + self.lock = threading.Lock() + self.threshold_image_lock = threading.Lock() + self.color_image_lock = threading.Lock() + self.cont = 0 + self.iteration = 0 + + # self.previous_timestamp = 0 + # self.previous_image = 0 + + self.previous_v = None + self.previous_w = None + self.previous_w_normalized = None + self.suddenness_distance = [] + + client = carla.Client('localhost', 2000) + client.set_timeout(10.0) # seconds + world = client.get_world() + time.sleep(3) + self.vehicle = world.get_actors().filter('vehicle.*')[0] + + time.sleep(2) + + def update_frame(self, frame_id, data): + """Update the information to be shown in one of the GUI's frames. + + Arguments: + frame_id {str} -- Id of the frame that will represent the data + data {*} -- Data to be shown in the frame. Depending on the type of frame (rgbimage, laser, pose3d, etc) + """ + self.handler.update_frame(frame_id, data) + + def update_pose(self, pose_data): + self.handler.update_pose3d(pose_data) + + def execute(self): + image = self.camera.getImage().data + image_1 = self.camera_1.getImage().data + image_2 = self.camera_2.getImage().data + image_3 = self.camera_3.getImage().data + + bird_eye_view_1 = self.bird_eye_view.getImage(self.vehicle) + + #print(self.bird_eye_view.getImage(self.vehicle)) + + #self.motors.sendW(w) + self.motors.sendV(1) + self.update_frame('frame_0', image) + self.update_frame('frame_1', image_1) + self.update_frame('frame_2', image_2) + self.update_frame('frame_3', image_3) + + #self.update_frame('frame_0', bird_eye_view_1) + + + self.update_pose(self.pose.getPose3d()) + #print(self.pose.getPose3d()) + diff --git a/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py b/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py new file mode 100644 index 00000000..1d75f9eb --- /dev/null +++ b/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py @@ -0,0 +1,151 @@ +#!/usr/bin/python +# -*- coding: utf-8 -*- +import csv +import cv2 +import math +import numpy as np +import threading +import time +import carla +from albumentations import ( + Compose, Normalize, RandomRain, RandomBrightness, RandomShadow, RandomSnow, RandomFog, RandomSunFlare +) +from utils.constants import DATASETS_DIR, ROOT_PATH + +import tensorflow as tf +gpus = tf.config.experimental.list_physical_devices('GPU') +for gpu in gpus: + tf.config.experimental.set_memory_growth(gpu, True) + +PRETRAINED_MODELS = "../models/" + +class Brain: + + def __init__(self, sensors, actuators, handler, config=None): + self.camera = sensors.get_camera('camera_0') + self.camera_1 = sensors.get_camera('camera_1') + self.camera_2 = sensors.get_camera('camera_2') + self.camera_3 = sensors.get_camera('camera_3') + + self.pose = sensors.get_pose3d('pose3d_0') + + self.bird_eye_view = sensors.get_bird_eye_view('bird_eye_view_0') + + self.motors = actuators.get_motor('motors_0') + self.handler = handler + self.config = config + + self.threshold_image = np.zeros((640, 360, 3), np.uint8) + self.color_image = np.zeros((640, 360, 3), np.uint8) + ''' + self.lock = threading.Lock() + self.threshold_image_lock = threading.Lock() + self.color_image_lock = threading.Lock() + ''' + self.cont = 0 + self.iteration = 0 + + # self.previous_timestamp = 0 + # self.previous_image = 0 + + self.previous_v = None + self.previous_w = None + self.previous_w_normalized = None + self.suddenness_distance = [] + + client = carla.Client('localhost', 2000) + client.set_timeout(10.0) # seconds + world = client.get_world() + time.sleep(10) + #print(world.get_actors()) + self.vehicle = world.get_actors().filter('vehicle.*')[0] + + model = '/home/jderobot/Documents/Projects/BehaviorMetrics/PlayingWithCARLA/models/20221104-143528_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_all_towns_vel_30_cp.h5' + self.net = tf.keras.models.load_model(model) + self.previous_speed = 0 + + + def update_frame(self, frame_id, data): + """Update the information to be shown in one of the GUI's frames. + + Arguments: + frame_id {str} -- Id of the frame that will represent the data + data {*} -- Data to be shown in the frame. Depending on the type of frame (rgbimage, laser, pose3d, etc) + """ + if data.shape[0] != data.shape[1]: + if data.shape[0] > data.shape[1]: + difference = data.shape[0] - data.shape[1] + extra_left, extra_right = int(difference/2), int(difference/2) + extra_top, extra_bottom = 0, 0 + else: + difference = data.shape[1] - data.shape[0] + extra_left, extra_right = 0, 0 + extra_top, extra_bottom = int(difference/2), int(difference/2) + + + data = np.pad(data, ((extra_top, extra_bottom), (extra_left, extra_right), (0, 0)), mode='constant', constant_values=0) + + self.handler.update_frame(frame_id, data) + + def update_pose(self, pose_data): + self.handler.update_pose3d(pose_data) + + def execute(self): + #print(self.vehicle.get_location()) + image = self.camera.getImage().data + image_1 = self.camera_1.getImage().data + image_2 = self.camera_2.getImage().data + image_3 = self.camera_3.getImage().data + + bird_eye_view_1 = self.bird_eye_view.getImage(self.vehicle) + + #print(self.bird_eye_view.getImage(self.vehicle)) + + #self.update_frame('frame_0', image) + self.update_frame('frame_1', image_1) + self.update_frame('frame_2', image_2) + self.update_frame('frame_3', image_3) + + self.update_frame('frame_0', bird_eye_view_1) + + self.update_pose(self.pose.getPose3d()) + + image_shape=(50, 150) + img_base = cv2.resize(bird_eye_view_1, image_shape) + + AUGMENTATIONS_TEST = Compose([ + Normalize() + ]) + image = AUGMENTATIONS_TEST(image=img_base) + img = image["image"] + + #velocity_dim = np.full((150, 50), 0.5) + velocity_dim = np.full((150, 50), self.previous_speed/30) + new_img_vel = np.dstack((img, velocity_dim)) + img = new_img_vel + + img = np.expand_dims(img, axis=0) + prediction = self.net.predict(img, verbose=0) + throttle = prediction[0][0] + steer = prediction[0][1] * (1 - (-1)) + (-1) + break_command = prediction[0][2] + speed = self.vehicle.get_velocity() + vehicle_speed = 3.6 * math.sqrt(speed.x**2 + speed.y**2 + speed.z**2) + self.previous_speed = vehicle_speed + + if vehicle_speed > 300: + self.motors.sendThrottle(0) + self.motors.sendSteer(steer) + self.motors.sendBrake(0) + else: + if vehicle_speed < 2: + self.motors.sendThrottle(1.0) + self.motors.sendSteer(0.0) + self.motors.sendBrake(0) + else: + self.motors.sendThrottle(throttle) + self.motors.sendSteer(steer) + self.motors.sendBrake(0) + + + diff --git a/behavior_metrics/brains/CARLA/brain_carla_slow_and_turn.py b/behavior_metrics/brains/CARLA/brain_carla_slow_and_turn.py new file mode 100644 index 00000000..88e685d4 --- /dev/null +++ b/behavior_metrics/brains/CARLA/brain_carla_slow_and_turn.py @@ -0,0 +1,76 @@ +#!/usr/bin/python +# -*- coding: utf-8 -*- +import csv +import cv2 +import math +import numpy as np +import threading +import time +from albumentations import ( + Compose, Normalize, RandomRain, RandomBrightness, RandomShadow, RandomSnow, RandomFog, RandomSunFlare +) +from utils.constants import DATASETS_DIR, ROOT_PATH + + + +GENERATED_DATASETS_DIR = ROOT_PATH + '/' + DATASETS_DIR + + +class Brain: + + def __init__(self, sensors, actuators, handler, config=None): + self.camera = sensors.get_camera('camera_0') + self.camera_1 = sensors.get_camera('camera_1') + self.camera_2 = sensors.get_camera('camera_2') + self.camera_3 = sensors.get_camera('camera_3') + + self.pose = sensors.get_pose3d('pose3d_0') + + self.motors = actuators.get_motor('motors_0') + self.handler = handler + self.config = config + + self.threshold_image = np.zeros((640, 360, 3), np.uint8) + self.color_image = np.zeros((640, 360, 3), np.uint8) + self.lock = threading.Lock() + self.threshold_image_lock = threading.Lock() + self.color_image_lock = threading.Lock() + self.cont = 0 + self.iteration = 0 + + # self.previous_timestamp = 0 + # self.previous_image = 0 + + self.previous_v = None + self.previous_w = None + self.previous_w_normalized = None + self.suddenness_distance = [] + + time.sleep(2) + + def update_frame(self, frame_id, data): + """Update the information to be shown in one of the GUI's frames. + + Arguments: + frame_id {str} -- Id of the frame that will represent the data + data {*} -- Data to be shown in the frame. Depending on the type of frame (rgbimage, laser, pose3d, etc) + """ + self.handler.update_frame(frame_id, data) + + def update_pose(self, pose_data): + self.handler.update_pose3d(pose_data) + + def execute(self): + image = self.camera.getImage().data + image_1 = self.camera_1.getImage().data + image_2 = self.camera_2.getImage().data + image_3 = self.camera_3.getImage().data + + self.motors.sendW(1) + self.motors.sendV(0.5) + self.update_frame('frame_0', image) + self.update_frame('frame_1', image_1) + self.update_frame('frame_2', image_2) + self.update_frame('frame_3', image_3) + self.update_pose(self.pose.getPose3d()) + #print(self.pose.getPose3d()) \ No newline at end of file diff --git a/behavior_metrics/brains/brains_handler.py b/behavior_metrics/brains/brains_handler.py index 3aaaf09e..743b78a6 100755 --- a/behavior_metrics/brains/brains_handler.py +++ b/behavior_metrics/brains/brains_handler.py @@ -39,8 +39,12 @@ def load_brain(self, path, model=None): if robot_type == 'f1rl': from utils import environment - environment.close_gazebo() + environment.close_ros_and_simulators() exec(open(self.brain_path).read()) + elif robot_type == 'CARLA': + module = importlib.import_module(import_name) + Brain = getattr(module, 'Brain') + self.active_brain = Brain(self.sensors, self.actuators, handler=self, config=self.config) else: if import_name in sys.modules: # for reloading sake del sys.modules[import_name] diff --git a/behavior_metrics/configs/CARLA_launch_files/town_01.launch b/behavior_metrics/configs/CARLA_launch_files/town_01.launch new file mode 100644 index 00000000..2889db83 --- /dev/null +++ b/behavior_metrics/configs/CARLA_launch_files/town_01.launch @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behavior_metrics/configs/CARLA_launch_files/town_01_no_gui.launch b/behavior_metrics/configs/CARLA_launch_files/town_01_no_gui.launch new file mode 100644 index 00000000..04d4cc90 --- /dev/null +++ b/behavior_metrics/configs/CARLA_launch_files/town_01_no_gui.launch @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behavior_metrics/configs/CARLA_launch_files/town_02.launch b/behavior_metrics/configs/CARLA_launch_files/town_02.launch new file mode 100644 index 00000000..d7cabeab --- /dev/null +++ b/behavior_metrics/configs/CARLA_launch_files/town_02.launch @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behavior_metrics/configs/CARLA_launch_files/town_02_no_gui.launch b/behavior_metrics/configs/CARLA_launch_files/town_02_no_gui.launch new file mode 100644 index 00000000..46ccb37c --- /dev/null +++ b/behavior_metrics/configs/CARLA_launch_files/town_02_no_gui.launch @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behavior_metrics/configs/CARLA_launch_files/town_03.launch b/behavior_metrics/configs/CARLA_launch_files/town_03.launch new file mode 100644 index 00000000..8e98014b --- /dev/null +++ b/behavior_metrics/configs/CARLA_launch_files/town_03.launch @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behavior_metrics/configs/CARLA_launch_files/town_03_no_gui.launch b/behavior_metrics/configs/CARLA_launch_files/town_03_no_gui.launch new file mode 100644 index 00000000..783a0da6 --- /dev/null +++ b/behavior_metrics/configs/CARLA_launch_files/town_03_no_gui.launch @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behavior_metrics/configs/CARLA_launch_files/town_04.launch b/behavior_metrics/configs/CARLA_launch_files/town_04.launch new file mode 100644 index 00000000..a2c7cc87 --- /dev/null +++ b/behavior_metrics/configs/CARLA_launch_files/town_04.launch @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behavior_metrics/configs/CARLA_launch_files/town_04_no_gui.launch b/behavior_metrics/configs/CARLA_launch_files/town_04_no_gui.launch new file mode 100644 index 00000000..dd063c87 --- /dev/null +++ b/behavior_metrics/configs/CARLA_launch_files/town_04_no_gui.launch @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behavior_metrics/configs/CARLA_launch_files/town_05.launch b/behavior_metrics/configs/CARLA_launch_files/town_05.launch new file mode 100644 index 00000000..f29b5fd6 --- /dev/null +++ b/behavior_metrics/configs/CARLA_launch_files/town_05.launch @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behavior_metrics/configs/CARLA_launch_files/town_05_no_gui.launch b/behavior_metrics/configs/CARLA_launch_files/town_05_no_gui.launch new file mode 100644 index 00000000..44911f9f --- /dev/null +++ b/behavior_metrics/configs/CARLA_launch_files/town_05_no_gui.launch @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behavior_metrics/configs/CARLA_launch_files/town_06.launch b/behavior_metrics/configs/CARLA_launch_files/town_06.launch new file mode 100644 index 00000000..0191f223 --- /dev/null +++ b/behavior_metrics/configs/CARLA_launch_files/town_06.launch @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behavior_metrics/configs/CARLA_launch_files/town_06_no_gui.launch b/behavior_metrics/configs/CARLA_launch_files/town_06_no_gui.launch new file mode 100644 index 00000000..3338db1c --- /dev/null +++ b/behavior_metrics/configs/CARLA_launch_files/town_06_no_gui.launch @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behavior_metrics/configs/CARLA_launch_files/town_07.launch b/behavior_metrics/configs/CARLA_launch_files/town_07.launch new file mode 100644 index 00000000..01a09cf4 --- /dev/null +++ b/behavior_metrics/configs/CARLA_launch_files/town_07.launch @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behavior_metrics/configs/CARLA_launch_files/town_07_no_gui.launch b/behavior_metrics/configs/CARLA_launch_files/town_07_no_gui.launch new file mode 100644 index 00000000..4e4ac5e0 --- /dev/null +++ b/behavior_metrics/configs/CARLA_launch_files/town_07_no_gui.launch @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behavior_metrics/configs/default_carla.yml b/behavior_metrics/configs/default_carla.yml new file mode 100644 index 00000000..928d55be --- /dev/null +++ b/behavior_metrics/configs/default_carla.yml @@ -0,0 +1,62 @@ +Behaviors: + Robot: + Sensors: + Cameras: + Camera_0: + Name: 'camera_0' + Topic: '/carla/ego_vehicle/rgb_front/image' + Camera_1: + Name: 'camera_1' + Topic: '/carla/ego_vehicle/rgb_view/image' + Camera_2: + Name: 'camera_2' + Topic: '/carla/ego_vehicle/semantic_segmentation_front/image' + Camera_3: + Name: 'camera_3' + Topic: '/carla/ego_vehicle/dvs_front/image' + Pose3D: + Pose3D_0: + Name: 'pose3d_0' + Topic: '/carla/ego_vehicle/odometry' + BirdEyeView: + BirdEyeView_0: + Name: 'bird_eye_view_0' + Topic: '' + Actuators: + CARLA_Motors: + Motors_0: + Name: 'motors_0' + Topic: '/carla/ego_vehicle/vehicle_control_cmd' + MaxV: 3 + MaxW: 0.3 + + BrainPath: 'brains/CARLA/brain_carla_bird_eye_deep_learning.py' + PilotTimeCycle: 50 + Parameters: + ImageTranform: '' + Type: 'CARLA' + Simulation: + World: /home/jderobot/Documents/Projects/BehaviorMetrics/behavior_metrics/configs/CARLA_launch_files/town_03.launch + Dataset: + In: '/tmp/my_bag.bag' + Out: '' + Stats: + Out: './' + PerfectLap: './perfect_bags/lap-simple-circuit.bag' + Layout: + Frame_0: + Name: frame_0 + Geometry: [1, 1, 1, 1] + Data: rgbimage + Frame_1: + Name: frame_1 + Geometry: [0, 1, 1, 1] + Data: rgbimage + Frame_2: + Name: frame_2 + Geometry: [0, 2, 1, 1] + Data: rgbimage + Frame_3: + Name: frame_3 + Geometry: [1, 2, 1, 1] + Data: rgbimage diff --git a/behavior_metrics/configs/default_carla_multiple.yml b/behavior_metrics/configs/default_carla_multiple.yml new file mode 100644 index 00000000..27564420 --- /dev/null +++ b/behavior_metrics/configs/default_carla_multiple.yml @@ -0,0 +1,70 @@ +Behaviors: + Robot: + Sensors: + Cameras: + Camera_0: + Name: 'camera_0' + Topic: '/carla/ego_vehicle/rgb_front/image' + Camera_1: + Name: 'camera_1' + Topic: '/carla/ego_vehicle/rgb_view/image' + Camera_2: + Name: 'camera_2' + Topic: '/carla/ego_vehicle/semantic_segmentation_front/image' + Camera_3: + Name: 'camera_3' + Topic: '/carla/ego_vehicle/dvs_front/image' + Pose3D: + Pose3D_0: + Name: 'pose3d_0' + Topic: '/carla/ego_vehicle/odometry' + BirdEyeView: + BirdEyeView_0: + Name: 'bird_eye_view_0' + Topic: '' + Actuators: + CARLA_Motors: + Motors_0: + Name: 'motors_0' + Topic: '/carla/ego_vehicle/vehicle_control_cmd' + MaxV: 3 + MaxW: 0.3 + + BrainPath: ['brains/CARLA/brain_carla_bird_eye_deep_learning.py', 'brains/CARLA/brain_carla_bird_eye_deep_learning.py'] + PilotTimeCycle: 50 + Parameters: + ImageTranform: '' + Type: 'CARLA' + Experiment: + Name: "Experiment name" + Description: "Experiment description" + Timeout: [10, 10] # for each world! + Repetitions: 2 + Simulation: + World: [ + /home/jderobot/Documents/Projects/BehaviorMetrics/behavior_metrics/configs/CARLA_launch_files/town_01_no_gui.launch, + /home/jderobot/Documents/Projects/BehaviorMetrics/behavior_metrics/configs/CARLA_launch_files/town_01_no_gui.launch + ] + Dataset: + In: '/tmp/my_bag.bag' + Out: '' + Stats: + Out: './' + PerfectLap: './perfect_bags/lap-simple-circuit.bag' + Layout: + Frame_0: + Name: frame_0 + Geometry: [1, 1, 1, 1] + Data: rgbimage + Frame_1: + Name: frame_1 + Geometry: [0, 1, 1, 1] + Data: rgbimage + Frame_2: + Name: frame_2 + Geometry: [0, 2, 1, 1] + Data: rgbimage + Frame_3: + Name: frame_3 + Geometry: [1, 2, 1, 1] + Data: rgbimage diff --git a/behavior_metrics/driver_carla.py b/behavior_metrics/driver_carla.py new file mode 100644 index 00000000..cf473baf --- /dev/null +++ b/behavior_metrics/driver_carla.py @@ -0,0 +1,142 @@ +import argparse +import os +import sys +import threading +import time +import rospy + +from pilot_carla import PilotCarla +from ui.tui.main_view import TUI +from utils import environment +from utils.colors import Colors +from utils.configuration import Config +from utils.controller_carla import ControllerCarla +from utils.logger import logger +from utils.tmp_world_generator import tmp_world_generator + + +def check_args(argv): + """Function that handles argument checking and parsing. + + Arguments: + argv {list} -- list of arguments from command line. + + Returns: + dict -- dictionary with the detected configuration. + """ + parser = argparse.ArgumentParser(description='Neural Behaviors Suite', + epilog='Enjoy the program! :)') + + parser.add_argument('-c', + '--config', + type=str, + action='append', + required=True, + help='{}Path to the configuration file in YML format.{}'.format( + Colors.OKBLUE, Colors.ENDC)) + + group = parser.add_mutually_exclusive_group(required=True) + group.add_argument('-g', + '--gui', + action='store_true', + help='{}Load the GUI (Graphic User Interface). Requires PyQt5 installed{}'.format( + Colors.OKBLUE, Colors.ENDC)) + + group.add_argument('-t', + '--tui', + action='store_true', + help='{}Load the TUI (Terminal User Interface). Requires npyscreen installed{}'.format( + Colors.OKBLUE, Colors.ENDC)) + + group.add_argument('-s', + '--script', + action='store_true', + help='{}Run Behavior Metrics as script{}'.format( + Colors.OKBLUE, Colors.ENDC)) + + parser.add_argument('-r', + '--random', + action='store_true', + help='{}Run Behavior Metrics F1 with random spawning{}'.format( + Colors.OKBLUE, Colors.ENDC)) + + args = parser.parse_args() + + config_data = {'config': None, 'gui': None, 'tui': None, 'script': None, 'random': False} + if args.config: + config_data['config'] = [] + for config_file in args.config: + if not os.path.isfile(config_file): + parser.error('{}No such file {} {}'.format(Colors.FAIL, config_file, Colors.ENDC)) + + config_data['config'] = args.config + + if args.gui: + config_data['gui'] = args.gui + + if args.tui: + config_data['tui'] = args.tui + + if args.script: + config_data['script'] = args.script + + if args.random: + config_data['random'] = args.random + + return config_data + +def main_win(configuration, controller): + """shows the Qt main window of the application + + Arguments: + configuration {Config} -- configuration instance for the application + controller {Controller} -- controller part of the MVC model of the application + """ + try: + from PyQt5.QtWidgets import QApplication + from ui.gui.views_controller import ParentWindow, ViewsController + + app = QApplication(sys.argv) + main_window = ParentWindow() + + views_controller = ViewsController(main_window, configuration, controller) + views_controller.show_main_view(True) + + main_window.show() + + app.exec_() + except Exception as e: + logger.error(e) + +def main(): + """Main function for the app. Handles creation and destruction of every element of the application.""" + + config_data = check_args(sys.argv) + app_configuration = Config(config_data['config'][0]) + if not config_data['script']: + environment.launch_env(app_configuration.current_world, carla_simulator=True) + controller = ControllerCarla() + + # Launch control + pilot = PilotCarla(app_configuration, controller, app_configuration.brain_path) + pilot.daemon = True + pilot.start() + logger.info('Executing app') + main_win(app_configuration, controller) + logger.info('closing all processes...') + pilot.kill_event.set() + environment.close_ros_and_simulators() + else: + for world_counter, world in enumerate(app_configuration.current_world): + for brain_counter, brain in enumerate(app_configuration.brain_path): + for repetition_counter in range(app_configuration.experiment_repetitions): + print("python3 script_driver_carla.py -c configs/default_carla_multiple.yml -s -world_counter " + str(world_counter) + " -brain_counter " + str(brain_counter) + " -repetition_counter " + str(repetition_counter)) + os.system("python3 script_manager_carla.py -c configs/default_carla_multiple.yml -s -world_counter " + str(world_counter) + " -brain_counter " + str(brain_counter) + " -repetition_counter " + str(repetition_counter)) + print("thread finished...exiting") + + logger.info('DONE! Bye, bye :)') + + +if __name__ == '__main__': + main() + sys.exit(0) diff --git a/behavior_metrics/driver.py b/behavior_metrics/driver_gazebo.py similarity index 89% rename from behavior_metrics/driver.py rename to behavior_metrics/driver_gazebo.py index fbe4703c..f5a81b38 100644 --- a/behavior_metrics/driver.py +++ b/behavior_metrics/driver_gazebo.py @@ -19,12 +19,12 @@ import sys import threading -from pilot import Pilot +from pilot_gazebo import PilotGazebo from ui.tui.main_view import TUI -from utils import environment, script_manager +from utils import environment, script_manager_gazebo from utils.colors import Colors from utils.configuration import Config -from utils.controller import Controller +from utils.controller_gazebo import ControllerGazebo from utils.logger import logger from utils.tmp_world_generator import tmp_world_generator @@ -112,7 +112,7 @@ def conf_window(configuration): configuration {Config} -- configuration instance for the application Keyword Arguments: - controller {Controller} -- controller part of the MVC of the application (default: {None}) + controller {ControllerGazebo} -- controller part of the MVC of the application (default: {None}) """ try: @@ -137,7 +137,7 @@ def main_win(configuration, controller): Arguments: configuration {Config} -- configuration instance for the application - controller {Controller} -- controller part of the MVC model of the application + controller {ControllerGazebo} -- controller part of the MVC model of the application """ try: from PyQt5.QtWidgets import QApplication @@ -165,7 +165,7 @@ def main(): app_configuration = Config(config) # Create controller of model-view - controller = Controller() + controller = ControllerGazebo() # If there's no config, configure the app through the GUI if app_configuration.empty and config_data['gui']: @@ -194,7 +194,7 @@ def main(): if not config_data['script']: # Launch control - pilot = Pilot(app_configuration, controller, app_configuration.brain_path) + pilot = PilotGazebo(app_configuration, controller, app_configuration.brain_path) pilot.daemon = True pilot.start() logger.info('Executing app') @@ -208,11 +208,11 @@ def main(): # When window is closed or keypress for quit is detected, quit gracefully. logger.info('closing all processes...') pilot.kill_event.set() - environment.close_gazebo() + environment.close_ros_and_simulators() else: - script_manager.run_brains_worlds(app_configuration, controller, randomize=config_data['random']) + script_manager_gazebo.run_brains_worlds(app_configuration, controller, randomize=config_data['random']) logger.info('closing all processes...') - environment.close_gazebo() + environment.close_ros_and_simulators() logger.info('DONE! Bye, bye :)') diff --git a/behavior_metrics/pilot_carla.py b/behavior_metrics/pilot_carla.py new file mode 100644 index 00000000..94df00b9 --- /dev/null +++ b/behavior_metrics/pilot_carla.py @@ -0,0 +1,277 @@ +#!/usr/bin/env python +""" This module is responsible for handling the logic of the robot and its current brain. + +This program is free software: you can redistribute it and/or modify it under +the terms of the GNU General Public License as published by the Free Software +Foundation, either version 3 of the License, or (at your option) any later +version. +This program is distributed in the hope that it will be useful, but WITHOUT +ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +You should have received a copy of the GNU General Public License along with +this program. If not, see . +""" + +import threading +import time +import rospy +import subprocess + +from datetime import datetime +from brains.brains_handler import Brains +from robot.actuators import Actuators +from robot.sensors import Sensors +from utils.logger import logger +from utils.constants import MIN_EXPERIMENT_PERCENTAGE_COMPLETED +from rosgraph_msgs.msg import Clock +from carla_msgs.msg import CarlaControl + +import numpy as np + +__author__ = 'fqez' +__contributors__ = [] +__license__ = 'GPLv3' + + +class PilotCarla(threading.Thread): + """This class handles the robot and its brain. + + This class called PilotCarla that handles the initialization of the robot sensors and actuators and the + brain that will control the robot. The main logic consists of an infinite loop called every 60 milliseconds that + invoke an action from the brain. + + Attributes: + controller {utils.controller.Controller} -- Controller instance of the MVC of the application + configuration {utils.configuration.Config} -- Configuration instance of the application + sensors {robot.sensors.Sensors} -- Sensors instance of the robot + actuators {robot.actuators.Actuators} -- Actuators instance of the robot + brains {brains.brains_handler.Brains} -- Brains controller instance + """ + + def __init__(self, configuration, controller, brain_path): + """Constructor of the pilot class + + Arguments: + configuration {utils.configuration.Config} -- Configuration instance of the application + controller {utils.controller.Controller} -- Controller instance of the MVC of the application + """ + + self.controller = controller + self.controller.set_pilot(self) + self.configuration = configuration + self.stop_event = threading.Event() + self.kill_event = threading.Event() + threading.Thread.__init__(self, args=self.stop_event) + self.brain_path = brain_path + self.robot_type = self.brain_path.split("/")[-2] + self.sensors = None + self.actuators = None + self.brains = None + self.initialize_robot() + if self.robot_type == 'drone': + self.pose3d = self.brains.active_brain.getPose3d() + self.start_pose = np.array([self.pose3d[0], self.pose3d[1]]) + else: + self.pose3d = self.sensors.get_pose3d('pose3d_0') + self.start_pose = np.array([self.pose3d.getPose3d().x, self.pose3d.getPose3d().y]) + self.previous = datetime.now() + self.checkpoints = [] + self.metrics = {} + self.checkpoint_save = False + self.max_distance = 0.5 + self.execution_completed = False + self.stats_thread = threading.Thread(target=self.track_stats) + self.stats_thread.start() + self.ros_clock_time = 0 + self.real_time_factor = 0 + self.brain_iterations_real_time = [] + self.brain_iterations_simulated_time = [] + self.real_time_factors = [] + self.real_time_update_rate = 1000 + self.pilot_start_time = 0 + self.time_cycle = self.configuration.pilot_time_cycle + + def __wait_carla(self): + """Wait for simulator to be initialized""" + + self.stop_event.set() + + def initialize_robot(self): + """Initialize robot interfaces (sensors and actuators) and its brain from configuration""" + self.stop_interfaces() + if self.robot_type != 'drone': + self.actuators = Actuators(self.configuration.actuators) + self.sensors = Sensors(self.configuration.sensors) + if hasattr(self.configuration, 'experiment_model') and type(self.configuration.experiment_model) != list: + self.brains = Brains(self.sensors, self.actuators, self.brain_path, self.controller, + self.configuration.experiment_model, self.configuration.brain_kwargs) + else: + self.brains = Brains(self.sensors, self.actuators, self.brain_path, self.controller, + config=self.configuration.brain_kwargs) + self.__wait_carla() + + def stop_interfaces(self): + """Function that kill the current interfaces of the robot. For reloading purposes.""" + if self.sensors: + self.sensors.kill() + if self.actuators: + self.actuators.kill() + pass + + def run(self): + """Main loop of the class. Calls a brain action every self.time_cycle""" + "TODO: cleanup measure of ips" + it = 0 + ss = time.time() + self.brain_iterations_real_time = [] + self.brain_iterations_simulated_time = [] + self.real_time_factors = [] + self.sensors.get_camera('camera_0').total_frames = 0 + self.pilot_start_time = time.time() + + + control_pub = rospy.Publisher('/carla/control', CarlaControl, queue_size=1) + control_command = CarlaControl() + control_command.command = 1 + control_pub.publish(control_command) + + while not self.kill_event.is_set(): + if not self.stop_event.is_set(): + control_pub = rospy.Publisher('/carla/control', CarlaControl, queue_size=1) + control_command = CarlaControl() + control_command.command = 2 + control_pub.publish(control_command) + + start_time = datetime.now() + start_time_ros = self.ros_clock_time + self.execution_completed = False + try: + self.brains.active_brain.execute() + except AttributeError as e: + logger.warning('No Brain selected') + logger.error(e) + + dt = datetime.now() - start_time + ms = (dt.days * 24 * 60 * 60 + dt.seconds) * 1000 + dt.microseconds / 1000.0 + self.brain_iterations_real_time.append(ms / 1000) + elapsed = time.time() - ss + if elapsed < 1: + it += 1 + else: + ss = time.time() + it = 0 + if ms < self.time_cycle: + time.sleep((self.time_cycle - ms) / 1000.0) + self.real_time_factors.append(self.real_time_factor) + self.brain_iterations_simulated_time.append(self.ros_clock_time - start_time_ros) + self.execution_completed = True + self.kill() + logger.info('Pilot: pilot killed.') + + def stop(self): + """Pause the main loop""" + + self.stop_event.set() + + def play(self): + """Resume the main loop.""" + + if self.is_alive(): + self.stop_event.clear() + else: + self.start() + + def kill(self): + """Destroy the main loop. For exiting""" + self.stop_interfaces() + self.actuators.kill() + self.kill_event.set() + + def reload_brain(self, brain_path, model=None): + """Reload a brain specified by brain_path + + This function is useful if one wants to change the environment of the robot (simulated world). + + Arguments: + brain_path {str} -- Path to the brain module to load. + """ + self.brains.load_brain(brain_path, model=model) + + def finish_line(self): + pose = self.pose3d.getPose3d() + current_point = np.array([pose.x, pose.y]) + + dist = (self.start_pose - current_point) ** 2 + dist = np.sum(dist, axis=0) + dist = np.sqrt(dist) + if dist < self.max_distance: + return True + return False + + def calculate_metrics(self, experiment_metrics): + if hasattr(self.brains.active_brain, 'inference_times'): + self.brains.active_brain.inference_times = self.brains.active_brain.inference_times[10:-10] + mean_inference_time = sum(self.brains.active_brain.inference_times) / len(self.brains.active_brain.inference_times) + frame_rate = self.sensors.get_camera('camera_0').total_frames / experiment_metrics['experiment_total_simulated_time'] + gpu_inference = self.brains.active_brain.gpu_inference + real_time_update_rate = self.real_time_update_rate + first_image = self.brains.active_brain.first_image + logger.info('* Mean network inference time ---> ' + str(mean_inference_time) + 's') + logger.info('* Frame rate ---> ' + str(frame_rate) + 'fps') + else: + mean_inference_time = 0 + frame_rate = 0 + gpu_inference = False + real_time_update_rate = self.real_time_update_rate + first_image = None + logger.info('No deep learning based brain') + if self.brain_iterations_real_time and self.brain_iterations_simulated_time and self.brain_iterations_simulated_time: + mean_brain_iterations_simulated_time = sum(self.brain_iterations_simulated_time) / len(self.brain_iterations_simulated_time) + real_time_factor = sum(self.real_time_factors) / len(self.real_time_factors) + brain_iterations_frequency_simulated_time = 1 / mean_brain_iterations_simulated_time + target_brain_iterations_simulated_time = 1000 / self.time_cycle / round(real_time_factor, 2) + mean_brain_iterations_real_time = sum(self.brain_iterations_real_time) / len(self.brain_iterations_real_time) + brain_iterations_frequency_real_time = 1 / mean_brain_iterations_real_time + target_brain_iterations_real_time = 1 / (self.time_cycle / 1000) + suddenness_distance = sum(self.brains.active_brain.suddenness_distance) / len(self.brains.active_brain.suddenness_distance) + else: + mean_brain_iterations_real_time = 0 + mean_brain_iterations_simulated_time = 0 + real_time_factor = 0 + brain_iterations_frequency_simulated_time = 0 + target_brain_iterations_simulated_time = 0 + brain_iterations_frequency_real_time = 0 + target_brain_iterations_real_time = 0 + suddenness_distance = 0 + logger.info('* Brain iterations frequency simulated time ---> ' + str(brain_iterations_frequency_simulated_time) + 'it/s') + logger.info('* Target brain iterations simulated time -> ' + str(target_brain_iterations_simulated_time) + 'it/s') + logger.info('* Mean brain iterations real time ---> ' + str(mean_brain_iterations_real_time) + 's') + logger.info('* Brain iterations frequency real time ---> ' + str(brain_iterations_frequency_real_time) + 'it/s') + logger.info('* Target brain iterations real time -> ' + str(target_brain_iterations_real_time) + 'it/s') + logger.info('* Mean brain iterations simulated time ---> ' + str(mean_brain_iterations_simulated_time) + 's') + logger.info('* Mean brain iterations simulated time ---> ' + str(real_time_factor)) + logger.info('* Real time update rate ---> ' + str(real_time_update_rate)) + logger.info('* GPU inference ---> ' + str(gpu_inference)) + logger.info('* Suddenness distance ---> ' + str(suddenness_distance)) + logger.info('* Saving experiment ---> ' + str(hasattr(self.controller, 'experiment_metrics_filename'))) + experiment_metrics['brain_iterations_frequency_simulated_time'] = brain_iterations_frequency_simulated_time + experiment_metrics['target_brain_iterations_simulated_time'] = target_brain_iterations_simulated_time + experiment_metrics['mean_brain_iterations_real_time'] = mean_brain_iterations_real_time + experiment_metrics['brain_iterations_frequency_real_time'] = brain_iterations_frequency_real_time + experiment_metrics['target_brain_iterations_real_time'] = target_brain_iterations_real_time + experiment_metrics['mean_inference_time'] = mean_inference_time + experiment_metrics['frame_rate'] = frame_rate + experiment_metrics['gpu_inference'] = gpu_inference + experiment_metrics['mean_brain_iterations_simulated_time'] = mean_brain_iterations_simulated_time + experiment_metrics['real_time_factor'] = real_time_factor + experiment_metrics['real_time_update_rate'] = real_time_update_rate + experiment_metrics['suddenness_distance'] = suddenness_distance + logger.info('Saving metrics to ROS bag') + return experiment_metrics, first_image + + def clock_callback(self, clock_data): + #(clock_data.clock.to_sec()) + self.ros_clock_time = clock_data.clock.to_sec() + + def track_stats(self): + self.clock_subscriber = rospy.Subscriber("/clock", Clock, self.clock_callback) diff --git a/behavior_metrics/pilot.py b/behavior_metrics/pilot_gazebo.py similarity index 95% rename from behavior_metrics/pilot.py rename to behavior_metrics/pilot_gazebo.py index d6d5c862..fde7cf7a 100644 --- a/behavior_metrics/pilot.py +++ b/behavior_metrics/pilot_gazebo.py @@ -32,7 +32,7 @@ __license__ = 'GPLv3' -class Pilot(threading.Thread): +class PilotGazebo(threading.Thread): """This class handles the robot and its brain. This class called Pilot that handles the initialization of the robot sensors and actuators and the @@ -93,16 +93,8 @@ def __init__(self, configuration, controller, brain_path): def __wait_gazebo(self): """Wait for gazebo to be initialized""" - # gazebo_ready = False self.stop_event.set() - # while not gazebo_ready: - # try: - # self.controller.pause_gazebo_simulation() - # gazebo_ready = True - # self.stop_event.clear() - # except Exception as ex: - # print(ex) def initialize_robot(self): """Initialize robot interfaces (sensors and actuators) and its brain from configuration""" diff --git a/behavior_metrics/robot/actuators.py b/behavior_metrics/robot/actuators.py index 03f1473b..53137b8f 100644 --- a/behavior_metrics/robot/actuators.py +++ b/behavior_metrics/robot/actuators.py @@ -12,7 +12,7 @@ this program. If not, see . """ -from .interfaces.motors import PublisherMotors +from .interfaces.motors import PublisherMotors, PublisherCARLAMotors __author__ = 'fqez' __contributors__ = [] @@ -36,9 +36,12 @@ def __init__(self, actuators_config): # Load motors motors_conf = actuators_config.get('Motors', None) + carla_motors_conf = actuators_config.get('CARLA_Motors', None) self.motors = None if motors_conf: self.motors = self.__create_actuator(motors_conf, 'motor') + elif carla_motors_conf: + self.motors = self.__create_actuator(carla_motors_conf, 'carla_motor') def __create_actuator(self, actuator_config, actuator_type): """Fill the motors dictionary with instances of the motors to control the robot""" @@ -49,14 +52,16 @@ def __create_actuator(self, actuator_config, actuator_type): topic = actuator_config[elem]['Topic'] vmax = actuator_config[elem]['MaxV'] wmax = actuator_config[elem]['MaxW'] - + if 'RL' in actuator_config[elem]: if actuator_config[elem]['RL'] == False: if actuator_type == 'motor': actuator_dict[name] = PublisherMotors(topic, vmax, wmax, 0, 0) else: if actuator_type == 'motor': - actuator_dict[name] = PublisherMotors(topic, vmax, wmax, 0, 0) + actuator_dict[name] = PublisherMotors(topic, vmax, wmax, 0, 0) + elif actuator_type == 'carla_motor': + actuator_dict[name] = PublisherCARLAMotors(topic, vmax, wmax, 0, 0) return actuator_dict def __get_actuator(self, actuator_name, actuator_type): diff --git a/behavior_metrics/robot/interfaces/birdeyeview.py b/behavior_metrics/robot/interfaces/birdeyeview.py new file mode 100644 index 00000000..88ab9cb7 --- /dev/null +++ b/behavior_metrics/robot/interfaces/birdeyeview.py @@ -0,0 +1,21 @@ +import carla +from carla_birdeye_view import BirdViewProducer, BirdViewCropType, PixelDimensions + +class BirdEyeView: + + def __init__(self): + client = carla.Client('localhost', 2000) + client.set_timeout(10.0) + self.birdview_producer = BirdViewProducer( + client, # carla.Client + target_size=PixelDimensions(width=100, height=300), + pixels_per_meter=10, + crop_type=BirdViewCropType.FRONT_AREA_ONLY + ) + + def getImage(self, vehicle): + birdview = self.birdview_producer.produce( + agent_vehicle=vehicle # carla.Actor (spawned vehicle) + ) + image = BirdViewProducer.as_rgb(birdview) + return image diff --git a/behavior_metrics/robot/interfaces/motors.py b/behavior_metrics/robot/interfaces/motors.py index 457e9a82..f5ec6dcb 100644 --- a/behavior_metrics/robot/interfaces/motors.py +++ b/behavior_metrics/robot/interfaces/motors.py @@ -3,6 +3,8 @@ import threading from .threadPublisher import ThreadPublisher +from carla_msgs.msg import CarlaEgoVehicleControl + def cmdvel2Twist(vel): @@ -17,7 +19,21 @@ def cmdvel2Twist(vel): return tw -class CMDVel (): +def cmdvel2CarlaEgoVehicleControl(vel): + vehicle_control = CarlaEgoVehicleControl() + vehicle_control.throttle = vel.throttle + vehicle_control.steer = vel.steer + vehicle_control.brake = vel.brake + vehicle_control.hand_brake = False + vehicle_control.reverse = False + vehicle_control.gear = 0 + vehicle_control.manual_gear_shift = False + + return vehicle_control + + + +class CMDVel(): def __init__(self): @@ -38,6 +54,26 @@ def __str__(self): s = s + "\n timeStamp: " + str(self.timeStamp) + "\n}" return s +class CARLAVel(): + + def __init__(self): + + self.throttle = 0.0 + self.steer = 0.0 + self.brake = 0.0 + self.hand_brake = False + self.reverse = False + self.gear = 0 + self.manual_gear_shift = False + + def __str__(self): + s = "CARLAVel: {\n throttle: " + str(self.throttle) + "\n steer: " + str(self.steer) + s = s + "\n brake: " + str(self.brake) + "\n hand_brake: " + str(self.hand_brake) + s = s + "\n reverse: " + str(self.reverse) + "\n gear: " + str(self.gear) + s = s + "\n manual_gear_shift: " + str(self.manual_gear_shift) + "\n}" + return s + + class PublisherMotors: @@ -118,3 +154,73 @@ def sendAZ(self, az): self.lock.acquire() self.data.az = az self.lock.release() + + +class PublisherCARLAMotors: + + def __init__(self, topic, maxV, maxW, v, w): + + self.maxW = maxW + self.maxV = maxV + self.v = v + self.w = w + self.topic = topic + self.data = CARLAVel() + self.pub = rospy.Publisher(self.topic, CarlaEgoVehicleControl, queue_size=1) + rospy.init_node("CARLAMotors") + self.lock = threading.Lock() + self.kill_event = threading.Event() + self.thread = ThreadPublisher(self, self.kill_event) + self.thread.daemon = True + self.start() + + def publish(self): + self.lock.acquire() + vehicle_control = cmdvel2CarlaEgoVehicleControl(self.data) + self.lock.release() + self.pub.publish(vehicle_control) + + def stop(self): + self.kill_event.set() + self.pub.unregister() + + def start(self): + + self.kill_event.clear() + self.thread.start() + + def getTopic(self): + return self.topic + + def getMaxW(self): + return self.maxW + + def getMaxV(self): + return self.maxV + + def sendVelocities(self, vel): + + self.lock.acquire() + self.data = vel + self.lock.release() + + def sendThrottle(self, throttle): + + self.lock.acquire() + self.data.throttle = throttle + self.lock.release() + self.throttle = throttle + + def sendSteer(self, steer): + + self.lock.acquire() + self.data.steer = steer + self.lock.release() + self.steer = steer + + def sendBrake(self, brake): + + self.lock.acquire() + self.data.brake = brake + self.lock.release() + self.brake = brake diff --git a/behavior_metrics/robot/sensors.py b/behavior_metrics/robot/sensors.py index 702e6821..08a5aeb9 100644 --- a/behavior_metrics/robot/sensors.py +++ b/behavior_metrics/robot/sensors.py @@ -15,6 +15,7 @@ from robot.interfaces.camera import ListenerCamera from robot.interfaces.laser import ListenerLaser from robot.interfaces.pose3d import ListenerPose3d +from robot.interfaces.birdeyeview import BirdEyeView __author__ = 'fqez' __contributors__ = [] @@ -54,6 +55,12 @@ def __init__(self, sensors_config): if pose3d_conf: self.pose3d = self.__create_sensor(pose3d_conf, 'pose3d') + # Load BirdEyeView + bird_eye_view_conf = sensors_config.get('BirdEyeView', None) + self.bird_eye_view = None + if bird_eye_view_conf: + self.bird_eye_view = self.__create_sensor(bird_eye_view_conf, 'bird_eye_view') + def __create_sensor(self, sensor_config, sensor_type): """Fill the sensor dictionary with instances of the sensor_type and sensor_config""" sensor_dict = {} @@ -66,6 +73,8 @@ def __create_sensor(self, sensor_config, sensor_type): sensor_dict[name] = ListenerLaser(topic) elif sensor_type == 'pose3d': sensor_dict[name] = ListenerPose3d(topic) + elif sensor_type == 'bird_eye_view': + sensor_dict[name] = BirdEyeView() return sensor_dict @@ -80,6 +89,8 @@ def __get_sensor(self, sensor_name, sensor_type): sensor = self.lasers[sensor_name] elif sensor_type == 'pose3d': sensor = self.pose3d[sensor_name] + elif sensor_type == 'bird_eye_view': + sensor = self.bird_eye_view[sensor_name] except KeyError: return "[ERROR] No existing camera with {} name.".format(sensor_name) @@ -118,6 +129,17 @@ def get_pose3d(self, pose_name): """ return self.__get_sensor(pose_name, 'pose3d') + def get_bird_eye_view(self, bird_eye_view_name): + """Retrieve an specific existing bird eye view + + Arguments: + bird_eye_view_name {str} -- Name of the birdeyeview to be retrieved + + Returns: + robot.interfaces.birdeyeview.BirdEyeView instance -- birdeyeview instance + """ + return self.__get_sensor(bird_eye_view_name, 'bird_eye_view') + def kill(self): """Destroy all the running sensors""" if self.cameras: diff --git a/behavior_metrics/script_manager_carla.py b/behavior_metrics/script_manager_carla.py new file mode 100644 index 00000000..7674cacb --- /dev/null +++ b/behavior_metrics/script_manager_carla.py @@ -0,0 +1,146 @@ +import argparse +import os +import sys +import threading +import time +import rospy + +from pilot_carla import PilotCarla +from ui.tui.main_view import TUI +from utils import environment +from utils.colors import Colors +from utils.configuration import Config +from utils.controller_carla import ControllerCarla +from utils.logger import logger +from utils.tmp_world_generator import tmp_world_generator + +def check_args(argv): + parser = argparse.ArgumentParser(description='Neural Behaviors Suite', + epilog='Enjoy the program! :)') + + parser.add_argument('-c', + '--config', + type=str, + action='append', + required=True, + help='{}Path to the configuration file in YML format.{}'.format( + Colors.OKBLUE, Colors.ENDC)) + + group = parser.add_mutually_exclusive_group(required=True) + group.add_argument('-g', + '--gui', + action='store_true', + help='{}Load the GUI (Graphic User Interface). Requires PyQt5 installed{}'.format( + Colors.OKBLUE, Colors.ENDC)) + + group.add_argument('-t', + '--tui', + action='store_true', + help='{}Load the TUI (Terminal User Interface). Requires npyscreen installed{}'.format( + Colors.OKBLUE, Colors.ENDC)) + + group.add_argument('-s', + '--script', + action='store_true', + help='{}Run Behavior Metrics as script{}'.format( + Colors.OKBLUE, Colors.ENDC)) + + parser.add_argument('-r', + '--random', + action='store_true', + help='{}Run Behavior Metrics F1 with random spawning{}'.format( + Colors.OKBLUE, Colors.ENDC)) + + parser.add_argument('-world_counter', + type=str, + action='append', + help='{}World counter{}'.format( + Colors.OKBLUE, Colors.ENDC)) + + parser.add_argument('-brain_counter', + type=str, + action='append', + help='{}Brain counter{}'.format( + Colors.OKBLUE, Colors.ENDC)) + + parser.add_argument('-repetition_counter', + type=str, + action='append', + help='{}Repetition counter{}'.format( + Colors.OKBLUE, Colors.ENDC)) + + args = parser.parse_args() + + config_data = {'config': None, 'gui': None, 'tui': None, 'script': None, 'random': False, 'world_counter': 0, 'brain_counter': 0, 'repetition_counter': 0} + if args.config: + config_data['config'] = [] + for config_file in args.config: + if not os.path.isfile(config_file): + parser.error('{}No such file {} {}'.format(Colors.FAIL, config_file, Colors.ENDC)) + + config_data['config'] = args.config + + if args.gui: + config_data['gui'] = args.gui + + if args.tui: + config_data['tui'] = args.tui + + if args.script: + config_data['script'] = args.script + + if args.random: + config_data['random'] = args.random + + if args.world_counter: + config_data['world_counter'] = args.world_counter + + if args.brain_counter: + config_data['brain_counter'] = args.brain_counter + + if args.repetition_counter: + config_data['repetition_counter'] = args.repetition_counter + + return config_data + +def main(): + config_data = check_args(sys.argv) + app_configuration = Config(config_data['config'][0]) + + world_counter = int(config_data['world_counter'][0]) + brain_counter = int(config_data['brain_counter'][0]) + repetition_counter = int(config_data['repetition_counter'][0]) + + print(world_counter, brain_counter, repetition_counter) + + world = app_configuration.current_world[world_counter] + brain = app_configuration.brain_path[brain_counter] + environment.launch_env(world, carla_simulator=True) + controller = ControllerCarla() + + # Launch control + pilot = PilotCarla(app_configuration, controller, brain) + pilot.daemon = True + pilot.start() + logger.info('Executing app') + controller.reload_brain(brain) + controller.resume_pilot() + controller.unpause_carla_simulation() + controller.record_metrics(app_configuration.stats_out, world_counter=world_counter, brain_counter=brain_counter, repetition_counter=repetition_counter) + + rospy.sleep(app_configuration.experiment_timeouts[world_counter]) + controller.stop_recording_metrics() + controller.pilot.stop() + controller.stop_pilot() + controller.pause_carla_simulation() + + logger.info('closing all processes...') + controller.pilot.kill() + environment.close_ros_and_simulators() + while not controller.pilot.execution_completed: + time.sleep(1) + + +if __name__ == '__main__': + main() + sys.exit(0) \ No newline at end of file diff --git a/behavior_metrics/show_plots.py b/behavior_metrics/show_plots.py index ac8c66b6..4c5662ef 100644 --- a/behavior_metrics/show_plots.py +++ b/behavior_metrics/show_plots.py @@ -7,7 +7,7 @@ from cv_bridge import CvBridge import numpy as np -from utils import metrics +from utils import metrics_gazebo from matplotlib.backends.qt_compat import QtWidgets from matplotlib.backends.backend_qt5agg import (FigureCanvas, NavigationToolbar2QT as NavigationToolbar) from PyQt5.QtWidgets import QApplication, QWidget, QLabel, QVBoxLayout, QPushButton, QGridLayout @@ -222,7 +222,7 @@ def show_metrics(bags, bags_checkpoints, bags_metadata, bags_experiment_data, fi elif bags_metadata[x]['world'] == 'montmelo_line.launch': perfect_lap_path = 'perfect_bags/lap-montmelo.bag' - perfect_lap_checkpoints, circuit_diameter = metrics.read_perfect_lap_rosbag(perfect_lap_path) + perfect_lap_checkpoints, circuit_diameter = metrics_gazebo.read_perfect_lap_rosbag(perfect_lap_path) experiment_metrics = bags_experiment_data[x] experiment_metrics['experiment_metrics'] = experiment_metrics experiments_metrics.append(experiment_metrics) diff --git a/behavior_metrics/ui/gui/resources/assets/carla_black.png b/behavior_metrics/ui/gui/resources/assets/carla_black.png new file mode 100644 index 00000000..2a800a01 Binary files /dev/null and b/behavior_metrics/ui/gui/resources/assets/carla_black.png differ diff --git a/behavior_metrics/ui/gui/resources/assets/carla_light.png b/behavior_metrics/ui/gui/resources/assets/carla_light.png new file mode 100644 index 00000000..aee88575 Binary files /dev/null and b/behavior_metrics/ui/gui/resources/assets/carla_light.png differ diff --git a/behavior_metrics/ui/gui/resources/worlds.json b/behavior_metrics/ui/gui/resources/worlds.json index 08aa3b43..ad7c0eb3 100644 --- a/behavior_metrics/ui/gui/resources/worlds.json +++ b/behavior_metrics/ui/gui/resources/worlds.json @@ -270,5 +270,12 @@ "world": "/opt/jderobot/share/jderobot/gazebo/launch/roomba_1_house.launch", "description": "Description" } + ], + "CARLA": [ + { + "name": "Town01", + "world": "/home/jderobot/carla-ros-bridge/catkin_ws/src/ros-bridge/carla_ros_bridge/launch/carla_ros_bridge_with_example_ego_vehicle.launch", + "description": "Description" + } ] } diff --git a/behavior_metrics/ui/gui/views/stats_window.py b/behavior_metrics/ui/gui/views/stats_window.py index afba9bd5..82ef693a 100644 --- a/behavior_metrics/ui/gui/views/stats_window.py +++ b/behavior_metrics/ui/gui/views/stats_window.py @@ -62,3 +62,29 @@ def __init__(self, parent=None, controller=None): self.layout.addWidget(self.circuit_diameter_label) wid.setLayout(self.layout) + + +class CARLAStatsWindow(QMainWindow): + def __init__(self, parent=None, controller=None): + super(CARLAStatsWindow, self).__init__(parent) + + self.controller = controller + self.setWindowTitle("Metrics") + wid = QWidget(self) + self.setCentralWidget(wid) + + self.layout = QVBoxLayout() + self.completed_distance_label = QLabel("Completed distance -> " + str(self.controller.experiment_metrics['completed_distance']) + " m") + self.layout.addWidget(self.completed_distance_label) + self.average_speed_label = QLabel("Average speed -> " + str(self.controller.experiment_metrics['average_speed']) + " km/h") + self.layout.addWidget(self.average_speed_label) + self.experiment_total_real_time_label = QLabel("Experiment total real time -> " + str(self.controller.experiment_metrics['experiment_total_real_time']) + ' s') + self.layout.addWidget(self.experiment_total_real_time_label) + self.experiment_total_simulated_time_label = QLabel("Experiment total simulated time -> " + str(self.controller.experiment_metrics['experiment_total_simulated_time']) + ' s') + self.layout.addWidget(self.experiment_total_simulated_time_label) + self.collisions_label = QLabel("Collisions -> " + str(self.controller.experiment_metrics['collisions'])) + self.layout.addWidget(self.collisions_label) + self.lane_invasions_label = QLabel("Lane invasions -> " + str(self.controller.experiment_metrics['lane_invasions'])) + self.layout.addWidget(self.lane_invasions_label) + + wid.setLayout(self.layout) \ No newline at end of file diff --git a/behavior_metrics/ui/gui/views/toolbar.py b/behavior_metrics/ui/gui/views/toolbar.py index 0d365826..afd5328a 100644 --- a/behavior_metrics/ui/gui/views/toolbar.py +++ b/behavior_metrics/ui/gui/views/toolbar.py @@ -25,10 +25,10 @@ QPushButton, QScrollArea, QSpacerItem, QVBoxLayout, QWidget) -from ui.gui.views.stats_window import StatsWindow +from ui.gui.views.stats_window import StatsWindow, CARLAStatsWindow from ui.gui.views.logo import Logo from ui.gui.views.social import SocialMedia -from utils import constants, environment +from utils import constants, environment, controller_carla __author__ = 'fqez' __contributors__ = [] @@ -201,6 +201,8 @@ def enterEvent(self, event): # self.setStyleSheet('background-color: black') if self.id == 'gzcli': self.setPixmap(QPixmap(':/assets/gazebo_dark.png')) + elif self.id == 'carlacli': + self.setPixmap(QPixmap(':/assets/carla_black.png')) elif self.id == 'play_record_dataset' or self.id == 'sim': if self.active: self.setPixmap(QPixmap(':/assets/pause_dark.png')) @@ -215,6 +217,8 @@ def leaveEvent(self, event): # self.setStyleSheet('background-color: rgb(0, 0, 0, 0,)') if self.id == 'gzcli': self.setPixmap(QPixmap(':/assets/gazebo_light.png')) + elif self.id == 'carlacli': + self.setPixmap(QPixmap(':/assets/carla_light.png')) elif self.id == 'play_record_dataset' or self.id == 'sim': if self.active: self.setPixmap(QPixmap(':/assets/pause.png')) @@ -530,10 +534,17 @@ def create_simulation_gb(self): start_pause_simulation_label.setToolTip('Start/Pause the simulation') reset_simulation = ClickableLabel('reset', 40, QPixmap(':/assets/reload.png'), parent=self) reset_simulation.setToolTip('Reset the simulation') - show_gzclient = ClickableLabel('gzcli', 40, QPixmap(':/assets/gazebo_light.png'), parent=self) - show_gzclient.setToolTip('Open/Close simulator window') + if type(self.controller) == controller_carla.ControllerCarla: + carla_image = ClickableLabel('carlacli', 40, QPixmap(':/assets/carla_light.png'), parent=self) + carla_image.setToolTip('Open/Close simulator window') + else: + show_gzclient = ClickableLabel('gzcli', 40, QPixmap(':/assets/gazebo_light.png'), parent=self) + show_gzclient.setToolTip('Open/Close simulator window') pause_reset_layout = QHBoxLayout() - pause_reset_layout.addWidget(show_gzclient, alignment=Qt.AlignRight) + if type(self.controller) == controller_carla.ControllerCarla: + pause_reset_layout.addWidget(carla_image, alignment=Qt.AlignRight) + else: + pause_reset_layout.addWidget(show_gzclient, alignment=Qt.AlignRight) pause_reset_layout.addWidget(start_pause_simulation_label, alignment=Qt.AlignCenter) pause_reset_layout.addWidget(reset_simulation, alignment=Qt.AlignLeft) @@ -585,7 +596,10 @@ def start_recording_stats(self): self.recording_stats_animation_label.start_animation() self.recording_stats_label.show() self.recording_stats_animation_label.show() - self.controller.record_metrics(filename, dirname) + if type(self.controller) == controller_carla.ControllerCarla: + self.controller.record_metrics(dirname) + else: + self.controller.record_metrics(filename, dirname) else: self.stats_hint_label.setText('Select a directory to save stats first!') self.stats_hint_label.show() @@ -599,7 +613,10 @@ def stop_recording_stats(self): self.recording_stats_label.hide() self.controller.stop_recording_metrics() - dialog = StatsWindow(self, self.controller) + if type(self.controller) == controller_carla.ControllerCarla: + dialog = CARLAStatsWindow(self, self.controller) + else: + dialog = StatsWindow(self, self.controller) dialog.show() def selection_change_brain(self, i): @@ -650,7 +667,10 @@ def select_directory_dialog(self): def reset_simulation(self): """Callback that handles simulation resetting""" - self.controller.reset_gazebo_simulation() + if type(self.controller) == controller_carla.ControllerCarla: + self.controller.reset_carla_simulation() + else: + self.controller.reset_gazebo_simulation() def pause_simulation(self): """Callback that handles simulation pausing""" @@ -664,7 +684,10 @@ def pause_simulation(self): self.confirm_brain.setStyleSheet('color: white') self.controller.pause_pilot() - self.controller.pause_gazebo_simulation() + if type(self.controller) == controller_carla.ControllerCarla: + self.controller.pause_carla_simulation() + else: + self.controller.pause_gazebo_simulation() def resume_simulation(self): """Callback that handles simulation resuming""" @@ -684,7 +707,10 @@ def resume_simulation(self): # save to configuration self.configuration.brain_path = brains_path + self.configuration.robot_type + '/' + brain - self.controller.unpause_gazebo_simulation() + if type(self.controller) == controller_carla.ControllerCarla: + self.controller.unpause_carla_simulation() + else: + self.controller.unpause_gazebo_simulation() def load_brain(self): """Callback that handles brain reloading""" diff --git a/behavior_metrics/utils/controller_carla.py b/behavior_metrics/utils/controller_carla.py new file mode 100644 index 00000000..68449e43 --- /dev/null +++ b/behavior_metrics/utils/controller_carla.py @@ -0,0 +1,299 @@ +#!/usr/bin/env python + +"""This module contains the controller of the application. + +This application is based on a type of software architecture called Model View Controller. This is the controlling part +of this architecture (controller), which communicates the logical part (model) with the user interface (view). + +This program is free software: you can redistribute it and/or modify it under +the terms of the GNU General Public License as published by the Free Software +Foundation, either version 3 of the License, or (at your option) any later +version. +This program is distributed in the hope that it will be useful, but WITHOUT +ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +You should have received a copy of the GNU General Public License along with +this program. If not, see . +""" + +import shlex +import subprocess +import threading +import cv2 +import rospy +import os +import time +import rosbag +import json +import math + +from std_srvs.srv import Empty +from sensor_msgs.msg import Image +from cv_bridge import CvBridge +from datetime import datetime +from utils.logger import logger +from utils.constants import CIRCUITS_TIMEOUTS +from std_msgs.msg import String +from utils import metrics_carla +from carla_msgs.msg import CarlaLaneInvasionEvent +from carla_msgs.msg import CarlaCollisionEvent + +__author__ = 'sergiopaniego' +__contributors__ = [] +__license__ = 'GPLv3' + + +class ControllerCarla: + """This class defines the controller of the architecture, responsible of the communication between the logic (model) + and the user interface (view). + + Attributes: + data {dict} -- Data to be sent to the view. The key is a frame_if of the view and the value is the data to be + displayed. Depending on the type of data the frame handles (images, laser, etc) + pose3D_data -- Pose data to be sent to the view + recording {bool} -- Flag to determine if a rosbag is being recorded + """ + + def __init__(self): + """ Constructor of the class. """ + pass + self.__data_loc = threading.Lock() + self.__pose_loc = threading.Lock() + self.data = {} + self.pose3D_data = None + self.recording = False + self.cvbridge = CvBridge() + #self.collision_sub = rospy.Subscriber('/carla/ego_vehicle/collision', CarlaCollisionEvent, self.__collision_callback) + #self.lane_invasion_sub = rospy.Subscriber('/carla/ego_vehicle/lane_invasion', CarlaLaneInvasionEvent, self.__lane_invasion_callback) + + + def __collision_callback(self, data): + intensity = math.sqrt(data.normal_impulse.x**2 + data.normal_impulse.y**2 + data.normal_impulse.z**2) + logger.info('Collision with {} (impulse {})'.format(data.other_actor_id, intensity)) + + def __lane_invasion_callback(self, data): + text = [] + for marking in data.crossed_lane_markings: + if marking is CarlaLaneInvasionEvent.LANE_MARKING_OTHER: + text.append("Other") + elif marking is CarlaLaneInvasionEvent.LANE_MARKING_BROKEN: + text.append("Broken") + elif marking is CarlaLaneInvasionEvent.LANE_MARKING_SOLID: + text.append("Solid") + else: + text.append("Unknown ") + logger.info('Crossed line %s' % ' and '.join(text)) + + # GUI update + def update_frame(self, frame_id, data): + """Update the data to be retrieved by the view. + + This function is called by the logic to update the data obtained by the robot to a specific frame in GUI. + + Arguments: + frame_id {str} -- Identifier of the frame that will show the data + data {dict} -- Data to be shown + """ + try: + with self.__data_loc: + self.data[frame_id] = data + except Exception as e: + logger.info(e) + + def get_data(self, frame_id): + """Function to collect data retrieved by the robot for an specific frame of the GUI + + This function is called by the view to get the last updated data to be shown in the GUI. + + Arguments: + frame_id {str} -- Identifier of the frame. + + Returns: + data -- Depending on the caller frame could be image data, laser data, etc. + """ + try: + with self.__data_loc: + data = self.data.get(frame_id, None) + except Exception: + pass + + return data + + def update_pose3d(self, data): + """Update the pose3D data retrieved from the robot + + Arguments: + data {pose3d} -- 3D position of the robot in the environment + """ + try: + with self.__pose_loc: + self.pose3D_data = data + except Exception: + pass + + def get_pose3D(self): + """Function to collect the pose3D data updated in `update_pose3d` function. + + This method is called from the view to collect the pose data and display it in GUI. + + Returns: + pose3d -- 3D position of the robot in the environment + """ + return self.pose3D_data + + # Simulation and dataset + def reset_carla_simulation(self): + logger.info("Restarting simulation") + + def pause_carla_simulation(self): + logger.info("Pausing simulation") + self.pilot.stop_event.set() + + def unpause_carla_simulation(self): + logger.info("Resuming simulation") + self.pilot.stop_event.clear() + + def record_rosbag(self, topics, dataset_name): + """Start the recording process of the dataset using rosbags + + Arguments: + topics {list} -- List of topics to be recorde + dataset_name {str} -- Path of the resulting bag file + """ + + if not self.recording: + logger.info("Recording bag at: {}".format(dataset_name)) + self.recording = True + command = "rosbag record -O " + dataset_name + " " + " ".join(topics) + " __name:=behav_bag" + command = shlex.split(command) + with open("logs/.roslaunch_stdout.log", "w") as out, open("logs/.roslaunch_stderr.log", "w") as err: + self.rosbag_proc = subprocess.Popen(command, stdout=out, stderr=err) + else: + logger.info("Rosbag already recording") + self.stop_record() + + def stop_record(self): + """Stop the rosbag recording process.""" + if self.rosbag_proc and self.recording: + logger.info("Stopping bag recording") + self.recording = False + command = "rosnode kill /behav_bag" + command = shlex.split(command) + with open("logs/.roslaunch_stdout.log", "w") as out, open("logs/.roslaunch_stderr.log", "w") as err: + subprocess.Popen(command, stdout=out, stderr=err) + else: + logger.info("No bag recording") + + def reload_brain(self, brain, model=None): + """Helper function to reload the current brain from the GUI. + + Arguments: + brain {srt} -- Brain to be reloadaed. + """ + logger.info("Reloading brain... {}".format(brain)) + + self.pause_pilot() + self.pilot.reload_brain(brain, model) + + # Helper functions (connection with logic) + + def set_pilot(self, pilot): + self.pilot = pilot + + def stop_pilot(self): + self.pilot.kill_event.set() + + def pause_pilot(self): + self.pilot.stop_event.set() + + def resume_pilot(self): + self.start_time = datetime.now() + self.pilot.start_time = datetime.now() + self.pilot.stop_event.clear() + + def initialize_robot(self): + self.pause_pilot() + self.pilot.initialize_robot() + + + def record_metrics(self, metrics_record_dir_path, world_counter=None, brain_counter=None, repetition_counter=None): + logger.info("Recording metrics bag at: {}".format(metrics_record_dir_path)) + self.start_time = datetime.now() + if world_counter is not None: + current_world_head, current_world_tail = os.path.split(self.pilot.configuration.current_world[world_counter]) + else: + current_world_head, current_world_tail = os.path.split(self.pilot.configuration.current_world) + if brain_counter is not None: + current_brain_head, current_brain_tail = os.path.split(self.pilot.configuration.brain_path[brain_counter]) + else: + current_brain_head, current_brain_tail = os.path.split(self.pilot.configuration.brain_path) + self.experiment_metadata = { + 'world': current_world_tail, + 'brain_path': current_brain_tail, + 'robot_type': self.pilot.configuration.robot_type + } + if hasattr(self.pilot.configuration, 'experiment_model'): + if brain_counter is not None: + self.experiment_metadata['experiment_model'] = self.pilot.configuration.experiment_model[brain_counter] + else: + self.experiment_metadata['experiment_model'] = self.pilot.configuration.experiment_model + if hasattr(self.pilot.configuration, 'experiment_name'): + self.experiment_metadata['experiment_name'] = self.pilot.configuration.experiment_name + self.experiment_metadata['experiment_description'] = self.pilot.configuration.experiment_description + if hasattr(self.pilot.configuration, 'experiment_timeouts'): + self.experiment_metadata['experiment_timeout'] = self.pilot.configuration.experiment_timeouts[world_counter] + else: + self.experiment_metadata['experiment_timeout'] = CIRCUITS_TIMEOUTS[os.path.basename(self.experiment_metadata['world'])] * 1.1 + self.experiment_metadata['experiment_repetition'] = repetition_counter + + self.metrics_record_dir_path = metrics_record_dir_path + time_str = time.strftime("%Y%m%d-%H%M%S") + self.experiment_metrics_filename = time_str + '.bag' + topics = ['/carla/ego_vehicle/odometry', '/carla/ego_vehicle/collision', '/carla/ego_vehicle/lane_invasion', '/clock'] + command = "rosbag record -O " + self.experiment_metrics_filename + " " + " ".join(topics) + " __name:=behav_metrics_bag" + command = shlex.split(command) + with open("logs/.roslaunch_stdout.log", "w") as out, open("logs/.roslaunch_stderr.log", "w") as err: + self.proc = subprocess.Popen(command, stdout=out, stderr=err) + + def stop_recording_metrics(self): + logger.info("Stopping metrics bag recording") + end_time = time.time() + + command = "rosnode kill /behav_metrics_bag" + command = shlex.split(command) + with open("logs/.roslaunch_stdout.log", "w") as out, open("logs/.roslaunch_stderr.log", "w") as err: + subprocess.Popen(command, stdout=out, stderr=err) + + # Wait for rosbag file to be closed. Otherwise it causes error + while os.path.isfile(self.experiment_metrics_filename + '.active'): + pass + + self.experiment_metrics = metrics_carla.get_metrics(self.experiment_metrics_filename) + + try: + self.save_metrics() + except rosbag.bag.ROSBagException: + logger.info("Bag was empty, Try Again") + + logger.info("* Experiment total real time -> " + str(end_time - self.pilot.pilot_start_time) + ' s') + self.experiment_metrics['experiment_total_real_time'] = end_time - self.pilot.pilot_start_time + + time_str = time.strftime("%Y%m%d-%H%M%S") + + with open(time_str + '.json', 'w') as f: + json.dump(self.experiment_metrics, f) + logger.info("Metrics stored in JSON file") + + logger.info("Stopping metrics bag recording") + + + def save_metrics(self): + experiment_metadata_str = json.dumps(self.experiment_metadata) + experiment_metrics_str = json.dumps(self.experiment_metrics) + with rosbag.Bag(self.experiment_metrics_filename, 'a') as bag: + experiment_metadata_msg = String(data=experiment_metadata_str) + experiment_metrics_msg = String(data=experiment_metrics_str) + bag.write('/metadata', experiment_metadata_msg, rospy.Time(bag.get_end_time())) + bag.write('/experiment_metrics', experiment_metrics_msg, rospy.Time(bag.get_end_time())) + bag.close() + diff --git a/behavior_metrics/utils/controller.py b/behavior_metrics/utils/controller_gazebo.py similarity index 97% rename from behavior_metrics/utils/controller.py rename to behavior_metrics/utils/controller_gazebo.py index 36a513db..2217df10 100644 --- a/behavior_metrics/utils/controller.py +++ b/behavior_metrics/utils/controller_gazebo.py @@ -33,14 +33,14 @@ from utils.logger import logger from utils.constants import CIRCUITS_TIMEOUTS from std_msgs.msg import String -from utils import metrics +from utils import metrics_gazebo __author__ = 'fqez' __contributors__ = [] __license__ = 'GPLv3' -class Controller: +class ControllerGazebo: """This class defines the controller of the architecture, responsible of the communication between the logic (model) and the user interface (view). @@ -224,9 +224,9 @@ def stop_recording_metrics(self, pitch_error=False): while os.path.isfile(self.experiment_metrics_filename + '.active'): pass - perfect_lap_checkpoints, circuit_diameter = metrics.read_perfect_lap_rosbag(self.perfect_lap_filename) + perfect_lap_checkpoints, circuit_diameter = metrics_gazebo.read_perfect_lap_rosbag(self.perfect_lap_filename) if not pitch_error: - self.experiment_metrics = metrics.get_metrics(self.experiment_metrics_filename, perfect_lap_checkpoints, circuit_diameter) + self.experiment_metrics = metrics_gazebo.get_metrics(self.experiment_metrics_filename, perfect_lap_checkpoints, circuit_diameter) self.experiment_metrics, first_image = self.pilot.calculate_metrics(self.experiment_metrics) try: diff --git a/behavior_metrics/utils/environment.py b/behavior_metrics/utils/environment.py index 5e53518e..ecb78fd9 100644 --- a/behavior_metrics/utils/environment.py +++ b/behavior_metrics/utils/environment.py @@ -18,6 +18,7 @@ import subprocess import sys import time +import os from utils.logger import logger @@ -28,70 +29,124 @@ __license__ = 'GPLv3' -def launch_env(launch_file): +def launch_env(launch_file, carla_simulator=False): """Launch the environmet specified by the launch_file given in command line at launch time. Arguments: launch_file {str} -- path of the launch file to be executed """ - # close previous instances of gazebo if hanged. - close_gazebo() + # close previous instances of ROS and simulators if hanged. + close_ros_and_simulators() try: + if carla_simulator: + with open("/tmp/.carlalaunch_stdout.log", "w") as out, open("/tmp/.carlalaunch_stderr.log", "w") as err: + subprocess.Popen(["/home/jderobot/Documents/Projects/carla_simulator_0_9_13/CarlaUE4.sh", "-RenderOffScreen"], stdout=out, stderr=err) + #subprocess.Popen(["/home/jderobot/Documents/Projects/carla_simulator_0_9_13/CarlaUE4.sh", "-RenderOffScreen", "-quality-level=Low"], stdout=out, stderr=err) + logger.info("SimulatorEnv: launching simulator server.") + time.sleep(5) with open("/tmp/.roslaunch_stdout.log", "w") as out, open("/tmp/.roslaunch_stderr.log", "w") as err: - subprocess.Popen(["roslaunch", launch_file], stdout=out, stderr=err) - logger.info("GazeboEnv: launching gzserver.") + child = subprocess.Popen(["roslaunch", launch_file], stdout=out, stderr=err) + logger.info("SimulatorEnv: launching simulator server.") except OSError as oe: - logger.error("GazeboEnv: exception raised launching gzserver. {}".format(oe)) - close_gazebo() + logger.error("SimulatorEnv: exception raised launching simulator server. {}".format(oe)) + close_ros_and_simulators() sys.exit(-1) - # give gazebo some time to initialize + # give simulator some time to initialize time.sleep(5) -def close_gazebo(): - """Kill all the gazebo and ROS processes.""" +def close_ros_and_simulators(): + """Kill all the simulators and ROS processes.""" try: ps_output = subprocess.check_output(["ps", "-Af"]).decode('utf-8').strip("\n") except subprocess.CalledProcessError as ce: - logger.error("GazeboEnv: exception raised executing ps command {}".format(ce)) + logger.error("SimulatorEnv: exception raised executing ps command {}".format(ce)) sys.exit(-1) if ps_output.count('gzclient') > 0: try: subprocess.check_call(["killall", "-9", "gzclient"]) - logger.debug("GazeboEnv: gzclient killed.") + logger.debug("SimulatorEnv: gzclient killed.") except subprocess.CalledProcessError as ce: - logger.error("GazeboEnv: exception raised executing killall command for gzclient {}".format(ce)) + logger.error("SimulatorEnv: exception raised executing killall command for gzclient {}".format(ce)) if ps_output.count('gzserver') > 0: try: subprocess.check_call(["killall", "-9", "gzserver"]) - logger.debug("GazeboEnv: gzserver killed.") + logger.debug("SimulatorEnv: gzserver killed.") except subprocess.CalledProcessError as ce: - logger.error("GazeboEnv: exception raised executing killall command for gzserver {}".format(ce)) + logger.error("SimulatorEnv: exception raised executing killall command for gzserver {}".format(ce)) + + if ps_output.count('CarlaUE4.sh') > 0: + try: + subprocess.check_call(["killall", "-9", "CarlaUE4.sh"]) + logger.debug("SimulatorEnv: CARLA server killed.") + except subprocess.CalledProcessError as ce: + logger.error("SimulatorEnv: exception raised executing killall command for CARLA server {}".format(ce)) + + if ps_output.count('CarlaUE4-Linux-Shipping') > 0: + try: + subprocess.check_call(["killall", "-9", "CarlaUE4-Linux-Shipping"]) + logger.debug("SimulatorEnv: CarlaUE4-Linux-Shipping killed.") + except subprocess.CalledProcessError as ce: + logger.error("SimulatorEnv: exception raised executing killall command for CarlaUE4-Linux-Shipping {}".format(ce)) + + if ps_output.count('rosout') > 0: + try: + import rosnode + for node in rosnode.get_node_names(): + if node != '/carla_ros_bridge': + subprocess.check_call(["rosnode", "kill", node]) + + logger.debug("SimulatorEnv:rosout killed.") + except subprocess.CalledProcessError as ce: + logger.error("SimulatorEnv: exception raised executing killall command for rosout {}".format(ce)) + + if ps_output.count('bridge.py') > 0: + try: + os.system("ps -ef | grep 'bridge.py' | awk '{print $2}' | xargs kill -9") + logger.debug("SimulatorEnv:bridge.py killed.") + except subprocess.CalledProcessError as ce: + logger.error("SimulatorEnv: exception raised executing killall command for bridge.py {}".format(ce)) + except FileNotFoundError as ce: + logger.error("SimulatorEnv: exception raised executing killall command for bridge.py {}".format(ce)) if ps_output.count('rosmaster') > 0: try: subprocess.check_call(["killall", "-9", "rosmaster"]) - logger.debug("GazeboEnv: rosmaster killed.") + logger.debug("SimulatorEnv: rosmaster killed.") except subprocess.CalledProcessError as ce: - logger.error("GazeboEnv: exception raised executing killall command for rosmaster {}".format(ce)) + logger.error("SimulatorEnv: exception raised executing killall command for rosmaster {}".format(ce)) if ps_output.count('roscore') > 0: try: subprocess.check_call(["killall", "-9", "roscore"]) - logger.debug("GazeboEnv: roscore killed.") + logger.debug("SimulatorEnv: roscore killed.") except subprocess.CalledProcessError as ce: - logger.error("GazeboEnv: exception raised executing killall command for roscore {}".format(ce)) + logger.error("SimulatorEnv: exception raised executing killall command for roscore {}".format(ce)) if ps_output.count('px4') > 0: try: subprocess.check_call(["killall", "-9", "px4"]) - logger.debug("GazeboEnv: px4 killed.") + logger.debug("SimulatorEnv: px4 killed.") + except subprocess.CalledProcessError as ce: + logger.error("SimulatorEnv: exception raised executing killall command for px4 {}".format(ce)) + + if ps_output.count('roslaunch') > 0: + try: + subprocess.check_call(["killall", "-9", "roslaunch"]) + logger.debug("SimulatorEnv: roslaunch killed.") + except subprocess.CalledProcessError as ce: + logger.error("SimulatorEnv: exception raised executing killall command for roslaunch {}".format(ce)) + + if ps_output.count('rosout') > 0: + try: + subprocess.check_call(["killall", "-9", "rosout"]) + logger.debug("SimulatorEnv:rosout killed.") except subprocess.CalledProcessError as ce: - logger.error("GazeboEnv: exception raised executing killall command for px4 {}".format(ce)) + logger.error("SimulatorEnv: exception raised executing killall command for rosout {}".format(ce)) def is_gzclient_open(): @@ -104,7 +159,7 @@ def is_gzclient_open(): try: ps_output = subprocess.check_output(["ps", "-Af"], encoding='utf8').strip("\n") except subprocess.CalledProcessError as ce: - logger.error("GazeboEnv: exception raised executing ps command {}".format(ce)) + logger.error("SimulatorEnv: exception raised executing ps command {}".format(ce)) sys.exit(-1) return ps_output.count('gzclient') > 0 @@ -116,9 +171,9 @@ def close_gzclient(): if is_gzclient_open(): try: subprocess.check_call(["killall", "-9", "gzclient"]) - logger.debug("GazeboEnv: gzclient killed.") + logger.debug("SimulatorEnv: gzclient killed.") except subprocess.CalledProcessError as ce: - logger.error("GazeboEnv: exception raised executing killall command for gzclient {}".format(ce)) + logger.error("SimulatorEnv: exception raised executing killall command for gzclient {}".format(ce)) def open_gzclient(): @@ -128,6 +183,6 @@ def open_gzclient(): try: with open("/tmp/.roslaunch_stdout.log", "w") as out, open("/tmp/.roslaunch_stderr.log", "w") as err: subprocess.Popen(["gzclient"], stdout=out, stderr=err) - logger.debug("GazeboEnv: gzclient started.") + logger.debug("SimulatorEnv: gzclient started.") except subprocess.CalledProcessError as ce: - logger.error("GazeboEnv: exception raised executing gzclient {}".format(ce)) + logger.error("SimulatorEnv: exception raised executing gzclient {}".format(ce)) diff --git a/behavior_metrics/utils/metrics_carla.py b/behavior_metrics/utils/metrics_carla.py new file mode 100644 index 00000000..a813922f --- /dev/null +++ b/behavior_metrics/utils/metrics_carla.py @@ -0,0 +1,135 @@ +#!/usr/bin/env python + +"""This module contains the metrics manager. +This module is in charge of generating metrics for a brain execution. +This program is free software: you can redistribute it and/or modify it under +the terms of the GNU General Public License as published by the Free Software +Foundation, either version 3 of the License, or (at your option) any later +version. +This program is distributed in the hope that it will be useful, but WITHOUT +ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +You should have received a copy of the GNU General Public License along with +this program. If not, see . +""" + +import pandas as pd +import numpy as np +import shutil +import time +import os +import rosbag + +from datetime import datetime +from bagpy import bagreader +from utils.logger import logger + + +def circuit_distance_completed(checkpoints, lap_point): + previous_point = [] + diameter = 0 + for i, point in enumerate(checkpoints): + current_point = np.array([point['pose.pose.position.x'], point['pose.pose.position.y']]) + if i != 0: + dist = (previous_point - current_point) ** 2 + dist = np.sum(dist, axis=0) + dist = np.sqrt(dist) + diameter += dist + if point is lap_point: + break + previous_point = current_point + return diameter + + +def get_metrics(stats_filename): + empty_metrics = { + "completed_distance": 0, + "average_speed": 0, + "experiment_total_simulated_time": 0 + } + experiment_metrics = {} + + time_counter = 5 + while not os.path.exists(stats_filename): + time.sleep(1) + time_counter -= 1 + if time_counter <= 0: + ValueError(f"{stats_filename} isn't a file!") + return empty_metrics + + try: + bag_reader = bagreader(stats_filename) + except rosbag.bag.ROSBagException: + return empty_metrics + + csv_files = [] + for topic in bag_reader.topics: + data = bag_reader.message_by_topic(topic) + csv_files.append(data) + + data_file = stats_filename.split('.bag')[0] + '/carla-ego_vehicle-odometry.csv' + dataframe_pose = pd.read_csv(data_file) + checkpoints = [] + for index, row in dataframe_pose.iterrows(): + checkpoints.append(row) + + data_file = stats_filename.split('.bag')[0] + '/clock.csv' + dataframe_pose = pd.read_csv(data_file) + clock_points = [] + for index, row in dataframe_pose.iterrows(): + clock_points.append(row) + start_clock = clock_points[0] + seconds_start = start_clock['clock.secs'] + seconds_end = clock_points[len(clock_points) - 1]['clock.secs'] + + collision_points = [] + if '/carla/ego_vehicle/collision' in bag_reader.topics: + data_file = stats_filename.split('.bag')[0] + '/carla-ego_vehicle-collision.csv' + dataframe_collision = pd.read_csv(data_file) + for index, row in dataframe_collision.iterrows(): + collision_points.append(row) + + lane_invasion_points = [] + if '/carla/ego_vehicle/lane_invasion' in bag_reader.topics: + data_file = stats_filename.split('.bag')[0] + '/carla-ego_vehicle-lane_invasion.csv' + dataframe_lane_invasion = pd.read_csv(data_file, on_bad_lines='skip') + for index, row in dataframe_lane_invasion.iterrows(): + lane_invasion_points.append(row) + + if len(checkpoints) > 1: + experiment_metrics = get_distance_completed(experiment_metrics, checkpoints) + experiment_metrics = get_average_speed(experiment_metrics, seconds_start, seconds_end) + experiment_metrics = get_collisions(experiment_metrics, collision_points) + experiment_metrics = get_lane_invasions(experiment_metrics, lane_invasion_points) + experiment_metrics['experiment_total_simulated_time'] = seconds_end - seconds_start + logger.info('* Experiment total simulated time ---> ' + str(experiment_metrics['experiment_total_simulated_time']) + ' s') + shutil.rmtree(stats_filename.split('.bag')[0]) + return experiment_metrics + else: + return empty_metrics + + +def get_distance_completed(experiment_metrics, checkpoints): + end_point = checkpoints[len(checkpoints) - 1] + experiment_metrics['completed_distance'] = circuit_distance_completed(checkpoints, end_point) + logger.info('* Completed distance ---> ' + str(experiment_metrics['completed_distance']) + ' m') + return experiment_metrics + + +def get_average_speed(experiment_metrics, seconds_start, seconds_end): + if (seconds_end - seconds_start): + experiment_metrics['average_speed'] = (experiment_metrics['completed_distance'] / (seconds_end - seconds_start))* 3.6 + else: + experiment_metrics['average_speed'] = 0 + logger.info('* Average speed ---> ' + str(experiment_metrics['average_speed']) + ' km/h') + return experiment_metrics + +def get_collisions(experiment_metrics, collision_points): + experiment_metrics['collisions'] = len(collision_points) + logger.info('* Collisions ---> ' + str(experiment_metrics['collisions'])) + return experiment_metrics + +def get_lane_invasions(experiment_metrics, lane_invasion_points): + experiment_metrics['lane_invasions'] = len(lane_invasion_points) + logger.info('* Lane invasions ---> ' + str(experiment_metrics['lane_invasions'])) + return experiment_metrics diff --git a/behavior_metrics/utils/metrics.py b/behavior_metrics/utils/metrics_gazebo.py similarity index 100% rename from behavior_metrics/utils/metrics.py rename to behavior_metrics/utils/metrics_gazebo.py diff --git a/behavior_metrics/utils/script_manager.py b/behavior_metrics/utils/script_manager_gazebo.py similarity index 95% rename from behavior_metrics/utils/script_manager.py rename to behavior_metrics/utils/script_manager_gazebo.py index 322a3c94..32d0ab4a 100644 --- a/behavior_metrics/utils/script_manager.py +++ b/behavior_metrics/utils/script_manager_gazebo.py @@ -25,10 +25,10 @@ import numpy as np -from utils import metrics +from utils import metrics_gazebo from utils.logger import logger from utils.constants import MIN_EXPERIMENT_PERCENTAGE_COMPLETED, CIRCUITS_TIMEOUTS -from pilot import Pilot +from pilot_gazebo import PilotGazebo from utils.tmp_world_generator import tmp_world_generator from rosgraph_msgs.msg import Clock @@ -42,7 +42,7 @@ def run_brains_worlds(app_configuration, controller, randomize=False): tmp_world_generator(world, app_configuration.stats_perfect_lap[world_counter], app_configuration.real_time_update_rate, randomize=randomize, gui=False, launch=True) - pilot = Pilot(app_configuration, controller, app_configuration.brain_path[brain_counter]) + pilot = PilotGazebo(app_configuration, controller, app_configuration.brain_path[brain_counter]) pilot.daemon = True pilot.real_time_update_rate = app_configuration.real_time_update_rate controller.pilot.start() @@ -63,7 +63,7 @@ def run_brains_worlds(app_configuration, controller, randomize=False): world_counter=world_counter, brain_counter=brain_counter, repetition_counter=repetition_counter) - perfect_lap_checkpoints, circuit_diameter = metrics.read_perfect_lap_rosbag( + perfect_lap_checkpoints, circuit_diameter = metrics_gazebo.read_perfect_lap_rosbag( app_configuration.stats_perfect_lap[world_counter]) new_point = np.array([controller.pilot.sensors.get_pose3d('pose3d_0').getPose3d().x, controller.pilot.sensors.get_pose3d('pose3d_0').getPose3d().y]) @@ -84,7 +84,7 @@ def run_brains_worlds(app_configuration, controller, randomize=False): controller.pilot.sensors.get_pose3d('pose3d_0').getPose3d().y]) if is_trapped(old_point, new_point): is_finished = True - elif metrics.is_finish_line(new_point, start_point): + elif metrics_gazebo.is_finish_line(new_point, start_point): is_finished = True elif previous_pitch != 0 and abs(controller.pilot.sensors.get_pose3d('pose3d_0').getPose3d().pitch - previous_pitch) > 0.2: diff --git a/behavior_metrics/utils/tmp_world_generator.py b/behavior_metrics/utils/tmp_world_generator.py index 95b81450..e2e395ec 100644 --- a/behavior_metrics/utils/tmp_world_generator.py +++ b/behavior_metrics/utils/tmp_world_generator.py @@ -28,14 +28,14 @@ import numpy as np -from utils import metrics +from utils import metrics_gazebo from utils import environment from utils.logger import logger def tmp_world_generator(current_world, stats_perfect_lap, real_time_update_rate, randomize=False, gui=False, launch=False): - environment.close_gazebo() + environment.close_ros_and_simulators() tree = ET.parse(current_world) root = tree.getroot() for child in root[0]: @@ -52,7 +52,7 @@ def tmp_world_generator(current_world, stats_perfect_lap, real_time_update_rate, tree = ET.parse(os.path.dirname(os.path.dirname(current_world)) + '/worlds/' + world_name) root = tree.getroot() - perfect_lap_checkpoints, circuit_diameter = metrics.read_perfect_lap_rosbag(stats_perfect_lap) + perfect_lap_checkpoints, circuit_diameter = metrics_gazebo.read_perfect_lap_rosbag(stats_perfect_lap) if randomize: random_index = random.randint(0, int(len(perfect_lap_checkpoints))) @@ -103,11 +103,11 @@ def tmp_world_generator(current_world, stats_perfect_lap, real_time_update_rate, try: with open("/tmp/.roslaunch_stdout.log", "w") as out, open("/tmp/.roslaunch_stderr.log", "w") as err: subprocess.Popen(["roslaunch", 'tmp_circuit.launch'], stdout=out, stderr=err) - logger.info("GazeboEnv: launching gzserver.") + logger.info("SimulatorEnv: launching gzserver.") except OSError as oe: - logger.error("GazeboEnv: exception raised launching gzserver. {}".format(oe)) - environment.close_gazebo() + logger.error("SimulatorEnv: exception raised launching gzserver. {}".format(oe)) + environment.close_ros_and_simulators() sys.exit(-1) - # give gazebo some time to initialize + # give simulator some time to initialize time.sleep(5) diff --git a/requirements.txt b/requirements.txt index 3aeb072c..38f7a718 100644 --- a/requirements.txt +++ b/requirements.txt @@ -34,3 +34,5 @@ tensorboard-plugin-wit==1.8.0 tensorflow-gpu==2.7.2 albumentations==1.0.3 protobuf==3.20.2 +carla_birdeye_view==1.1.1 +transforms3d==0.4.1 \ No newline at end of file