diff --git a/src/scenic/__main__.py b/src/scenic/__main__.py index d24a8d42b..2750340b2 100644 --- a/src/scenic/__main__.py +++ b/src/scenic/__main__.py @@ -47,6 +47,14 @@ intOptions.add_argument('-z', '--zoom', help='zoom expansion factor (default 1)', type=float, default=1) +# Recording options +recOptions = parser.add_argument_group('simulation recording options') +recOptions.add_argument('-r', '--record', help='enable recording of simulations', + action='store_true') +recOptions.add_argument('--sensors', help='path to sensor configuration file') +recOptions.add_argument('--recording_dir', + help='directory in which to save recorded data', default='./') + # Debugging options debugOpts = parser.add_argument_group('debugging options') debugOpts.add_argument('--show-params', help='show values of global parameters', @@ -116,6 +124,7 @@ if args.simulate: simulator = errors.callBeginningScenicTrace(scenario.getSimulator) + simulator.toggle_recording_sensors(args.record) def generateScene(): startTime = time.time() @@ -137,7 +146,9 @@ def runSimulation(scene): try: result = errors.callBeginningScenicTrace( lambda: simulator.simulate(scene, maxSteps=args.time, verbosity=args.verbosity, - maxIterations=args.max_sims_per_scene) + maxIterations=args.max_sims_per_scene, + save_dir=args.recording_dir, + sensor_config=args.sensors) ) except SimulationCreationError as e: if args.verbosity >= 1: diff --git a/src/scenic/core/simulators.py b/src/scenic/core/simulators.py index 9438bf5de..9683ba843 100644 --- a/src/scenic/core/simulators.py +++ b/src/scenic/core/simulators.py @@ -3,6 +3,7 @@ import types from collections import OrderedDict +import os from scenic.core.object_types import (enableDynamicProxyFor, setDynamicProxyFor, disableDynamicProxyFor) @@ -26,7 +27,7 @@ class Simulator: """A simulator which can import/execute scenes from Scenic.""" def simulate(self, scene, maxSteps=None, maxIterations=100, verbosity=0, - raiseGuardViolations=False): + raiseGuardViolations=False, save_dir='./', sensor_config=None): """Run a simulation for a given scene.""" # Repeatedly run simulations until we find one satisfying the requirements @@ -35,8 +36,23 @@ def simulate(self, scene, maxSteps=None, maxIterations=100, verbosity=0, iterations += 1 # Run a single simulation try: - simulation = self.createSimulation(scene, verbosity=verbosity) + simulation = self.createSimulation(scene, verbosity=verbosity, sensor_config=sensor_config) result = simulation.run(maxSteps) + if self.is_recording_sensors(): + # Create a subdirectory for the current simulation run + # Name of subdirectory is the next available index in save_dir + subdir_names = [dir_name for dir_name in os.listdir(save_dir) if os.path.isdir(os.path.join(save_dir, dir_name))] + subdir_names = [s for s in subdir_names if s.isdigit()] + subdir_idxes = sorted([int(s) for s in subdir_names]) + + if len(subdir_idxes) == 0: + next_subdir_idx = 0 + else: + next_subdir_idx = max(subdir_idxes) + 1 + + next_subdir_path = os.path.join(save_dir, str(next_subdir_idx)) + os.mkdir(next_subdir_path) + simulation.save_recordings(next_subdir_path) except (RejectSimulationException, RejectionException, dynamics.GuardViolation) as e: if verbosity >= 2: print(f' Rejected simulation {iterations} at time step ' @@ -58,6 +74,12 @@ def createSimulation(self, scene, verbosity=0): def destroy(self): pass + def toggle_recording_sensors(self, record): + raise NotImplementedError + + def is_recording_sensors(self): + return False + class Simulation: """A single simulation run, possibly in progress.""" diff --git a/src/scenic/simulators/carla/recording/README.md b/src/scenic/simulators/carla/recording/README.md new file mode 100644 index 000000000..8201e5615 --- /dev/null +++ b/src/scenic/simulators/carla/recording/README.md @@ -0,0 +1,180 @@ +# Scenic Data Generation Platform + +## Synthetic Dataset + +Our synthetic dataset, containing hundreds of simulations of Scenic programs, can be found [at this link](https://drive.google.com/drive/folders/18SrqL2q7PyMfaS0oKAFqoc6hVasXS20I?usp=sharing). + +If you wish to generate your own datasets, please follow the setup instructions below. If you're just looking to interact with our dataset above, feel free to skip to the API section. + +## Setup + +### Installing CARLA +* Download the latest release of CARLA. As of 10/6/20, this is located [here](https://github.com/carla-simulator/carla/releases/tag/0.9.10.1) + * Other releases can be found [here](https://github.com/carla-simulator/carla/releases) + * First, download “CARLA_0.9.10.1.tar.gz”. Unzip the contents of this folder into a directory of your choice. In this setup guide, we’ll unzip it into “~/carla” + * Download “AdditionalMaps_0.9.10.1.tar.gz”. Do not unzip this file. Rather, navigate to “~/carla” (the directory you unzipped CARLA into in the previous step), and place “AdditionalMaps_0.9.10.1.tar.gz” in the “Import” subdirectory. +* In the command line, cd into “~/carla” and run `./ImportAssets.sh` +* Try running `./CarlaUE4.sh -fps=15` from the “~/carla” directory. You should see a window pop up containing a 3D scene. +* The CARLA release contains a Python package for the API. To use this, you need to add the package to your terminal’s PYTHONPATH variable as follows: + * First, copy down the filepath of the Python package. The package should be located in “~/carla/PythonAPI/carla/dist”. Its name should be something like “carla-0.9.10-py3.7-linux-x86_64.egg” + * Open your “~/.bashrc” file in an editor. Create a new line with the following export statement: “export PYTHONPATH=/path/to/egg/file” + * Save and exit “~/.bashrc” and restart the terminal for the changes to take effect. To confirm that the package is on the PYTHONPATH, try the command “echo $PYTHONPATH” + +### Installing Scenic +* In a new terminal window, clone the current repository. +* In the command line, enter the repository and switch to the branch “dynamics2-recording” +* Run `poetry install` followed by `poetry shell` +* You’re now ready to run dynamic Scenic scripts! Here’s an example: `python -m scenic -S --time 200 --count 3 -m scenic.simulators.carla.model /path/to/scenic/script` + * This creates 3 simulations of the specified Scenic script, each of which runs for 200 time steps. Some example Scenic scripts are located in “examples/carla” + +## Dataset Generation + +To generate a synthetic dataset using Scenic, you need two things: a scenario configuration file and a sensor configuration file. + +### Scenario Configuration + +This file lets you configure which Scenic programs to simulate, how many times to simulate each program, how many steps to run each simulation for, and where to output the generated data. + +A sample scenario configuration file, which must be saved in the JSON format, is shown below. Feel free to change the list of scripts to reference any Scenic programs on your machine. + +``` +{ + "output_dir": "/path/to/output/dir", // dataset output directory + "simulations_per_scenario": 3, // number of simulations per Scenic program (script) + "time_per_simulation": 300, // time steps per simulation + "scripts": [ + "/path/to/scenario1", + "/path/to/scenario2" + ] +} +``` + +### Sensor Configuration + +This file is another JSON file that lets you configure the number, placement, and type of sensors with which to record. Right now, RGB video cameras and lidar sensors are supported (with ground-truth annotations). An example configuration file is as follows: + +``` +{ + [ + { + "name": "cam", + "type": "rgb", + "transform": { + "location": [0, 0, 2.4] + }, + "settings": { + "VIEW_WIDTH": 1280, + "VIEW_HEIGHT": 720, + "VIEW_FOV": 90 + } + }, + { + "name": "lidar", + "type": "lidar", + "transform": { + "location": [0, 0, 2.4] + }, + "settings": { + "PPS": 400000, + "UPPER_FOV": 15.0, + "LOWER_FOV": -25.0, + "RANGE": 40, + "ROTATION_FREQUENCY": 18.0 + } + } +] +``` + +In fact, this was the exact sensor configuration file that we used to generate our synthetic dataset. + +Now, to actually generate data using the configurations above, simply run: + +``` +python -m scenic.simulators.carla.recording --scenarios /path/to/scenario/config --sensors /path/to/sensor/config +``` + +Remember to enter `poetry shell` before running this command so that Scenic is properly set up as a module. Your dataset is on its way! + +## API + +Once you've either downloaded our provided dataset or generated one of your own, you can browse the data using our API: + +``` +from scenic.simulators.carla.recording import * +``` + +Load the sensor configuration file into a `SensorConfig` object: + +``` +sensor_config = SensorConfig('/path/to/sensor/config/file') +``` + +Load the generated dataset. The dataset directory here is the same as the "output_dir" specified in the scenario configuration file. If you're using our provided dataset, the dataset path will be the full directory path of "Town03", "Town05", "Town10", or "dense". + +``` +data = DataAPI('/path/to/dataset/directory', sensor_config) +``` + +Now you may browse the data as you please. The following example demonstrates how to draw 3D bounding boxes onto a frame selected from a particular simulation: + +``` +from scenic.simulators.carla.recording import * + +DATA_DIR = '/data/scenic_data_collection/Town03' +SENSOR_CONFIG_FILE = 'sensor_config.json' + +sensor_config = SensorConfig(SENSOR_CONFIG_FILE) + +data = DataAPI(DATA_DIR, sensor_config) + +sims = data.get_simulations() +sims = list(sims.values()) +sim = sims[0] + +frame = sim.get_frame(10) +draw_bbox_3d(frame['bboxes'], sensor_config.get('cam'), frame['cam']['rgb'], 'frame.jpg') +``` + +### API Documentation + +#### class DataAPI +* def get_simulations(self) + * Returns simulation data as a dictionary, with keys as the simulation name and values as `SimulationData` objects. + +#### class SimulationData +* def get_frame(self, frame_idx) + * Returns all sensor data recorded at a particular frame index (time step). For example, if we recorded with an RGB camera named "cam" and a lidar sensor named "lidar", then this function would return: +``` +{ + "bboxes": [list of 3D bounding boxes indexed by object type], + "cam": { + "rgb": image as numpy array, + "depth": image as numpy array, + "semantic": image as numpy array + }, + "lidar": { + "lidar": [list of lidar points] + } +} +``` + +#### class SensorConfig +* def get(self, sensor_name) + * Returns a dictionary object representing the sensor with name `sensor_name`. This sensor dictionary object should be used when working with the helper functions for drawing bounding boxes. + +The API includes utility functions to draw 2D and 3D bounding boxes onto any camera of choice, as well as to output a labeled point cloud that can be opened with software such as CloudCompare: + +* def draw_bbox_2d(bboxes, sensor, img, output_filepath) + * `bboxes`: a list of 3D bounding boxes as returned by `SimulationData.get_frame()` + * `sensor`: a sensor dictionary object as described above + * `img`: numpy array of the image frame to draw on + * `output_filepath`: where to output the final image + * This function draws 2D bounding boxes onto an image captured by a particular sensor + +The function for drawing 3D bounding boxes is similar. For outputting labeled point clouds, we have the following function: + +* def save_point_cloud(lidar_points, output_filepath) + * `lidar_points`: list of lidar points as returned by `SimulationData.get_frame()` + * `output_filepath`: where to output the point cloud (should have extension `.asc`) + +Ground-truth annotations contain semantic labels provided by CARLA, a complete description of which can be found [here](https://carla.readthedocs.io/en/latest/ref_sensors/#semantic-segmentation-camera) (scroll to the table of semantic tags). diff --git a/src/scenic/simulators/carla/recording/__init__.py b/src/scenic/simulators/carla/recording/__init__.py new file mode 100644 index 000000000..f77d5060c --- /dev/null +++ b/src/scenic/simulators/carla/recording/__init__.py @@ -0,0 +1 @@ +from .api import * \ No newline at end of file diff --git a/src/scenic/simulators/carla/recording/__main__.py b/src/scenic/simulators/carla/recording/__main__.py new file mode 100644 index 000000000..278e850bc --- /dev/null +++ b/src/scenic/simulators/carla/recording/__main__.py @@ -0,0 +1,36 @@ +import os +import subprocess +import argparse +import json + +parser = argparse.ArgumentParser(prog='scenic', add_help=False, + usage='scenic [-h | --help] [options] FILE [options]', + description='Sample from a Scenic scenario, optionally ' + 'running dynamic simulations.') + +parser.add_argument('--scenarios', help='path to scenario configuration file') +parser.add_argument('--sensors', help='path to sensor configuration file') + +args = parser.parse_args() + +with open(args.scenarios, 'r') as f: + scenario_config = json.load(f) + +for scenic_script_fpath in scenario_config['scripts']: + scenario_fname = os.path.basename(scenic_script_fpath) + scenario_name = scenario_fname.split('.')[0] + + scenario_recording_dir = os.path.join(scenario_config['output_dir'], scenario_name) + + if not os.path.isdir(scenario_recording_dir): + os.mkdir(scenario_recording_dir) + + for _ in range(scenario_config['simulations_per_scenario']): + command = 'python -m scenic -S --time {} --count 1 -r --sensors {} --recording_dir {} -m scenic.simulators.carla.model {}'.format( + scenario_config['time_per_simulation'], + args.sensors, + scenario_recording_dir, + scenic_script_fpath, + ) + + subprocess.call(command, shell=True) \ No newline at end of file diff --git a/src/scenic/simulators/carla/recording/api.py b/src/scenic/simulators/carla/recording/api.py new file mode 100644 index 000000000..49e1b191f --- /dev/null +++ b/src/scenic/simulators/carla/recording/api.py @@ -0,0 +1,284 @@ +import os +import cv2 +import json +import numpy as np + +import scenic.simulators.carla.utils.recording_utils as rec_utils + +from PIL import Image, ImageDraw + +class SensorConfig: + def __init__(self, sensor_config_file): + with open(sensor_config_file, 'r') as f: + sensor_data = json.load(f) + + self.sensors = {s['name']: s for s in sensor_data} + + def get(self, sensor_name): + return self.sensors[sensor_name] + + def get_sensors(self): + return self.sensors.values() + +class SimulationData: + def __init__(self, simulation_dir, sensor_config): + # Parameter for lazy fetching of data + self.have_fetched_data = False + + self.bboxes_fpath = os.path.join(simulation_dir, 'annotations', 'bboxes.json') + self.bbox_recording = None + + self.sensor_data = {} + + for sensor in sensor_config.get_sensors(): + sensor_dir = os.path.join(simulation_dir, sensor['name']) + + if sensor['type'] == 'rgb': + # Lazily retrieve simulation data from disk later, just store paths for now + sensor_data_paths = { + 'rgb': os.path.join(sensor_dir, 'rgb.mp4'), + 'depth': os.path.join(sensor_dir, 'depth.mp4'), + 'semantic': os.path.join(sensor_dir, 'semantic.mp4'), + } + + data = { + 'type': sensor['type'], + 'data_paths': sensor_data_paths, + } + + self.sensor_data[sensor['name']] = data + elif sensor['type'] == 'lidar': + sensor_data_paths = { + 'lidar': os.path.join(sensor_dir, 'lidar.json') + } + + data = { + 'type': sensor['type'], + 'data_paths': sensor_data_paths, + } + + self.sensor_data[sensor['name']] = data + + def fetch_data_from_disk(self): + self.bbox_recording = rec_utils.BBoxRecording.import_from_file(self.bboxes_fpath) + + for sensor in self.sensor_data.values(): + if sensor['type'] == 'rgb': + recordings = { + 'rgb': rec_utils.VideoRecording.import_from_file(sensor['data_paths']['rgb']), + 'depth': rec_utils.VideoRecording.import_from_file(sensor['data_paths']['depth']), + 'semantic': rec_utils.VideoRecording.import_from_file(sensor['data_paths']['semantic']), + } + + sensor['recordings'] = recordings + elif sensor['type'] == 'lidar': + recordings = { + 'lidar': rec_utils.FrameRecording.import_from_file(sensor['data_paths']['lidar']), + } + + sensor['recordings'] = recordings + + self.have_fetched_data = True + + def __len__(self): + if not self.have_fetched_data: + self.fetch_data_from_disk() + + return len(self.bbox_recording.frames) + + def __getitem__(self, frame_idx): + if not self.have_fetched_data: + self.fetch_data_from_disk() + + retval = {} + + retval['bboxes'] = self.bbox_recording.get_frame(frame_idx) + + for sensor_name, sensor in self.sensor_data.items(): + if sensor['type'] == 'rgb': + sensor_frame = { + 'rgb': sensor['recordings']['rgb'].get_frame(frame_idx), + 'depth': sensor['recordings']['depth'].get_frame(frame_idx), + 'semantic': sensor['recordings']['semantic'].get_frame(frame_idx), + } + + retval[sensor_name] = sensor_frame + elif sensor['type'] == 'lidar': + sensor_frame = { + 'lidar': sensor['recordings']['lidar'].get_frame(frame_idx) + } + + retval[sensor_name] = sensor_frame + + return retval + +class DataAPI: + def __init__(self, data_dir, sensor_config): + self.data_dir = data_dir + self.sensor_config = sensor_config + + # Index all simulations in data directory + self.simulation_data = {} + + scenario_dir_names = os.listdir(self.data_dir) + + for scen_dir_name in scenario_dir_names: + scenario_dir = os.path.join(self.data_dir, scen_dir_name) + simulation_dir_names = os.listdir(scenario_dir) + + for sim_dir_name in simulation_dir_names: + simulation_dir = os.path.join(scenario_dir, sim_dir_name) + + sim_data = SimulationData(simulation_dir, self.sensor_config) + + simulation_name = os.path.join(scen_dir_name, sim_dir_name) + self.simulation_data[simulation_name] = sim_data + + def get_simulations(self): + return self.simulation_data + +def get_bbox_3d_projected(bboxes, sensor): + retval = [] + + # Set up calibration matrix to be used for bounding box projection + calibration = np.identity(3) + VIEW_WIDTH = sensor['settings']['VIEW_WIDTH'] + VIEW_HEIGHT = sensor['settings']['VIEW_HEIGHT'] + VIEW_FOV = sensor['settings']['VIEW_FOV'] + calibration[0, 2] = VIEW_WIDTH / 2.0 + calibration[1, 2] = VIEW_HEIGHT / 2.0 + calibration[0, 0] = calibration[1, 1] = VIEW_WIDTH / (2.0 * np.tan(VIEW_FOV * np.pi / 360.0)) + + for obj_type in bboxes.keys(): + for bb in bboxes[obj_type]: + bbox_cords = np.array(bb) + + cords_x_y_z = np.transpose(bbox_cords) + cords_x_y_z_1 = np.vstack((cords_x_y_z, np.ones(cords_x_y_z.shape[1]))) + + # Transform bounding box coords from ego to sensor + sensor_to_ego = np.eye(4) + sensor_to_ego[0, 3] = sensor['transform']['location'][0] + sensor_to_ego[1, 3] = sensor['transform']['location'][1] + sensor_to_ego[2, 3] = sensor['transform']['location'][2] + ego_to_sensor = np.linalg.inv(sensor_to_ego) + + cords_x_y_z = np.dot(ego_to_sensor, cords_x_y_z_1)[:3] + + # Project 3D bounding box coordinates onto image plane + cords_y_minus_z_x = np.vstack([cords_x_y_z[1, :], -cords_x_y_z[2, :], cords_x_y_z[0, :]]) + camera_bbox = np.transpose(np.dot(calibration, cords_y_minus_z_x)) + camera_bbox = np.vstack([camera_bbox[:, 0] / camera_bbox[:, 2], camera_bbox[:, 1] / camera_bbox[:, 2], camera_bbox[:, 2]]) + camera_bbox = np.transpose(camera_bbox) + + # Only draw bbox if it's in front of sensor + if all(camera_bbox[:, 2] > 0): + retval.append(camera_bbox.tolist()) + + return retval + +def draw_bbox_3d(bboxes, sensor, img, output_filepath): + bboxes_projected = get_bbox_3d_projected(bboxes, sensor) + + frame = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) + im_pil = Image.fromarray(frame) + + draw = ImageDraw.Draw(im_pil) + + # Point radius + r = 6 + + FILL = 'red' + LINE_WIDTH = 4 + + for bbox in bboxes_projected: + for point in bbox: + draw.ellipse([point[0] - r, point[1] - r, point[0] + r, point[1] + r], fill=FILL) + + bottom_rect = [(point[0], point[1]) for point in bbox[:4]] + bottom_rect.append((bbox[0][0], bbox[0][1])) + draw.line(bottom_rect, fill=FILL, width=LINE_WIDTH) + + top_rect = [(point[0], point[1]) for point in bbox[4:]] + top_rect.append((bbox[4][0], bbox[4][1])) + draw.line(top_rect, fill=FILL, width=LINE_WIDTH) + + for point_idx in range(4): + bottom_pt = bbox[point_idx] + top_pt = bbox[point_idx + 4] + draw.line([bottom_pt[0], bottom_pt[1], top_pt[0], top_pt[1]], fill=FILL, width=LINE_WIDTH) + + im_pil.save(output_filepath) + +def get_bbox_2d(bboxes, sensor): + bboxes_projected = get_bbox_3d_projected(bboxes, sensor) + + retval = [] + + for bbox in bboxes_projected: + x_cords = [cord[0] for cord in bbox] + y_cords = [cord[1] for cord in bbox] + + min_x = min(x_cords) + min_y = min(y_cords) + + max_x = max(x_cords) + max_y = max(y_cords) + + retval.append([[min_x, min_y], [min_x, max_y], [max_x, max_y], [max_x, min_y]]) + + return retval + +def draw_bbox_2d(bboxes, sensor, img, output_filepath): + frame = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) + im_pil = Image.fromarray(frame) + draw = ImageDraw.Draw(im_pil) + + bboxes_2d = get_bbox_2d(bboxes, sensor) + + # Point radius + r = 6 + + FILL = 'red' + LINE_WIDTH = 4 + + for bbox in bboxes_2d: + for point in bbox: + draw.ellipse([point[0] - r, point[1] - r, point[0] + r, point[1] + r], fill=FILL) + + rect = [(point[0], point[1]) for point in bbox] + rect.append((bbox[0][0], bbox[0][1])) + draw.line(rect, fill=FILL, width=LINE_WIDTH) + + im_pil.save(output_filepath) + +def save_frame(img, output_filepath): + frame = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) + im_pil = Image.fromarray(frame) + im_pil.save(output_filepath) + +def save_point_cloud(lidar_points, output_filepath): + SEMANTIC_CLASS_TO_COLOR = { + 0: [0, 0, 0, 255], + 1: [70, 70, 70, 255], + 2: [190, 153, 153, 255], + 3: [250, 170, 160, 255], + 4: [220, 20, 60, 255], + 5: [153, 153, 153, 255], + 6: [157, 234, 50, 255], + 7: [128, 64, 128, 255], + 8: [244, 35, 232, 255], + 9: [107, 142, 35, 255], + 10: [0, 0, 142, 255], + 11: [102, 102, 156, 255], + 12: [220, 220, 0, 255] + } + + with open(output_filepath, 'w') as f: + for p in lidar_points: + if p[3] not in SEMANTIC_CLASS_TO_COLOR: + continue + semantic_color = SEMANTIC_CLASS_TO_COLOR[p[3]] + + # Convert to right-handed coordinate system + f.write('{} {} {} {} {} {}\n'.format(-p[0], p[1], p[2], semantic_color[0], semantic_color[1], semantic_color[2])) \ No newline at end of file diff --git a/src/scenic/simulators/carla/simulator.py b/src/scenic/simulators/carla/simulator.py index 4e1b9d7d9..f8c2a45e3 100644 --- a/src/scenic/simulators/carla/simulator.py +++ b/src/scenic/simulators/carla/simulator.py @@ -7,6 +7,7 @@ import math import os +import json from scenic.syntax.translator import verbosity if verbosity == 0: # suppress pygame advertisement at zero verbosity @@ -20,11 +21,15 @@ import scenic.simulators.carla.utils.utils as utils import scenic.simulators.carla.utils.visuals as visuals +from carla import ColorConverter as cc +import scenic.simulators.carla.utils.recording_utils as rec_utils +from scenic.simulators.carla.blueprints import * + class CarlaSimulator(DrivingSimulator): """Implementation of `Simulator` for CARLA.""" def __init__(self, carla_map, map_path, address='127.0.0.1', port=2000, timeout=10, - render=True, record='', timestep=0.1): + render=True, record='', record_sensors=False, timestep=0.1): super().__init__() verbosePrint('Connecting to CARLA...') self.client = carla.Client(address, port) @@ -51,13 +56,15 @@ def __init__(self, carla_map, map_path, address='127.0.0.1', port=2000, timeout= self.render = render # visualization mode ON/OFF self.record = record # whether to use the carla recorder + self.record_sensors = record_sensors # whether to record sensor data to disk self.scenario_number = 0 # Number of the scenario executed - def createSimulation(self, scene, verbosity=0): + def createSimulation(self, scene, verbosity=0, sensor_config=None): self.scenario_number += 1 return CarlaSimulation(scene, self.client, self.tm, self.timestep, - render=self.render, record=self.record, - scenario_number=self.scenario_number, verbosity=verbosity) + render=self.render, record=self.record, record_sensors=self.record_sensors, + scenario_number=self.scenario_number, verbosity=verbosity, + sensor_config=sensor_config) def destroy(self): settings = self.world.get_settings() @@ -68,9 +75,14 @@ def destroy(self): super().destroy() + def toggle_recording_sensors(self, record_sensors): + self.record_sensors = record_sensors + + def is_recording_sensors(self): + return self.record_sensors class CarlaSimulation(DrivingSimulation): - def __init__(self, scene, client, tm, timestep, render, record, scenario_number, verbosity=0): + def __init__(self, scene, client, tm, timestep, render, record, record_sensors, scenario_number, verbosity=0, sensor_config=None): super().__init__(scene, timestep=timestep, verbosity=verbosity) self.client = client self.world = self.client.get_world() @@ -111,6 +123,9 @@ def __init__(self, scene, client, tm, timestep, render, record, scenario_number, name = "{}/scenario{}.log".format(self.record, self.scenario_number) self.client.start_recorder(name) + self.record_sensors = record_sensors + self.bbox_buffer = [] + # Create Carla actors corresponding to Scenic objects self.ego = None for obj in self.objects: @@ -129,6 +144,111 @@ def __init__(self, scene, client, tm, timestep, render, record, scenario_number, self.cameraManager.set_sensor(camIndex) self.cameraManager.set_transform(self.camTransform) + if self.record_sensors: + if sensor_config is None: + raise RuntimeError('Must specify sensor configuration file when recording') + + with open(sensor_config, 'r') as f: + sensors = json.load(f) + + self.sensors = [] + + for sensor in sensors: + t_x, t_y, t_z = sensor['transform']['location'] + loc = carla.Location(x=t_x, y=t_y, z=t_z) + rot = carla.Rotation() + if 'rotation' in sensor['transform']: + yaw, pitch, roll = sensor['transform']['rotation'] + rot = carla.Rotation(yaw=yaw, pitch=pitch, roll=roll) + # TODO: support rotation + sensor_transform = carla.Transform(loc) + + if sensor['type'] == 'rgb': + VIEW_WIDTH = sensor['settings']['VIEW_WIDTH'] + VIEW_HEIGHT = sensor['settings']['VIEW_HEIGHT'] + VIEW_FOV = sensor['settings']['VIEW_FOV'] + + sensor_dict = { + 'name': sensor['name'], + 'type': sensor['type'], + 'rgb_buffer': [], + 'depth_buffer': [], + 'semantic_buffer': [] + } + self.sensors.append(sensor_dict) + + bp = self.world.get_blueprint_library().find('sensor.camera.rgb') + bp.set_attribute('image_size_x', str(VIEW_WIDTH)) + bp.set_attribute('image_size_y', str(VIEW_HEIGHT)) + bp.set_attribute('fov', str(VIEW_FOV)) + rgb_cam = self.world.spawn_actor(bp, sensor_transform, attach_to=carlaActor) + rgb_buffer = sensor_dict['rgb_buffer'] + rgb_cam.listen(lambda x: self.process_rgb_image(x, rgb_buffer)) + sensor_dict['rgb_cam_obj'] = rgb_cam + + bp = self.world.get_blueprint_library().find('sensor.camera.depth') + bp.set_attribute('image_size_x', str(VIEW_WIDTH)) + bp.set_attribute('image_size_y', str(VIEW_HEIGHT)) + bp.set_attribute('fov', str(VIEW_FOV)) + depth_cam = self.world.spawn_actor(bp, sensor_transform, attach_to=carlaActor) + depth_buffer = sensor_dict['depth_buffer'] + depth_cam.listen(lambda x: self.process_depth_image(x, depth_buffer)) + sensor_dict['depth_cam_obj'] = depth_cam + + bp = self.world.get_blueprint_library().find('sensor.camera.semantic_segmentation') + bp.set_attribute('image_size_x', str(VIEW_WIDTH)) + bp.set_attribute('image_size_y', str(VIEW_HEIGHT)) + semantic_cam = self.world.spawn_actor(bp, sensor_transform, attach_to=carlaActor) + semantic_buffer = sensor_dict['semantic_buffer'] + semantic_cam.listen(lambda x: self.process_semantic_image(x, semantic_buffer)) + sensor_dict['semantic_cam_obj'] = semantic_cam + + elif sensor['type'] == 'lidar': + sensor_dict = { + 'name': sensor['name'], + 'type': sensor['type'], + 'lidar_buffer': [] + } + self.sensors.append(sensor_dict) + + POINTS_PER_SECOND = sensor['settings']['PPS'] + UPPER_FOV = sensor['settings']['UPPER_FOV'] + LOWER_FOV = sensor['settings']['LOWER_FOV'] + RANGE = sensor['settings']['RANGE'] + ROTATION_FREQUENCY = sensor['settings']['ROTATION_FREQUENCY'] + + bp = self.world.get_blueprint_library().find('sensor.lidar.ray_cast_semantic') + bp.set_attribute('points_per_second', str(POINTS_PER_SECOND)) + bp.set_attribute('upper_fov', str(UPPER_FOV)) + bp.set_attribute('lower_fov', str(LOWER_FOV)) + bp.set_attribute('range', str(RANGE)) + bp.set_attribute('rotation_frequency', str(ROTATION_FREQUENCY)) + lidar_sensor = self.world.spawn_actor(bp, sensor_transform, attach_to=carlaActor) + lidar_sensor.listen(lambda x: self.process_lidar_data(x, sensor_dict['lidar_buffer'])) + sensor_dict['lidar_obj'] = lidar_sensor + + elif sensor['type'] == 'radar': + sensor_dict = { + 'name': sensor['name'], + 'type': sensor['type'], + 'radar_buffer': [] + } + self.sensors.append(sensor_dict) + + HORIZONTAL_FOV = sensor['settings']['HORIZONTAL_FOV'] + VERTICAL_FOV = sensor['settings']['VERTICAL_FOV'] + POINTS_PER_SECOND = sensor['settings']['PPS'] + RANGE = sensor['settings']['RANGE'] + + bp = self.world.get_blueprint_library().find('sensor.other.radar') + bp.set_attribute('horizontal_fov', str(HORIZONTAL_FOV)) + bp.set_attribute('vertical_fov', str(VERTICAL_FOV)) + bp.set_attribute('points_per_second', str(POINTS_PER_SECOND)) + bp.set_attribute('range', str(RANGE)) + radar_sensor = self.world.spawn_actor(bp, sensor_transform, attach_to=carlaActor) + radar_sensor.listen(lambda x: self.process_radar_data(x, sensor_dict['radar_buffer'])) + sensor_dict['radar_obj'] = radar_sensor + self.world.tick() ## allowing manualgearshift to take effect # TODO still need this? for obj in self.objects: @@ -197,6 +317,40 @@ def step(self): # Run simulation for one timestep self.world.tick() + if self.record_sensors: + actors = self.world.get_actors() + + classified_actors = { + 'car': [], + 'bicycle': [], + 'motorcycle': [], + 'truck': [], + 'pedestrian': [] + } + + for a in actors: + if a.type_id in carModels: + classified_actors['car'].append(a) + elif a.type_id in bicycleModels: + classified_actors['bicycle'].append(a) + elif a.type_id in motorcycleModels: + classified_actors['motorcycle'].append(a) + elif a.type_id in truckModels: + classified_actors['truck'].append(a) + elif a.type_id in walkerModels: + classified_actors['pedestrian'].append(a) + + bboxes = {} + curr_frame_idx = self.world.get_snapshot().frame + + for obj_class, obj_list in classified_actors.items(): + # Get bounding boxes relative to ego + bounding_boxes_3d = rec_utils.BBoxUtil.get_3d_bounding_boxes(obj_list, self.ego) + # Convert numpy matrices to lists + bboxes[obj_class] = [bbox.tolist() for bbox in bounding_boxes_3d] + + self.bbox_buffer.append((curr_frame_idx, bboxes)) + # Render simulation if self.render: # self.hud.tick(self.world, self.ego, self.displayClock) @@ -243,3 +397,120 @@ def destroy(self): self.world.tick() super().destroy() + + def process_rgb_image(self, image, buffer): + image.convert(cc.Raw) + buffer.append(image) + + def process_depth_image(self, image, buffer): + image.convert(cc.LogarithmicDepth) + buffer.append(image) + + def process_semantic_image(self, image, buffer): + image.convert(cc.CityScapesPalette) + buffer.append(image) + + def process_lidar_data(self, lidar_data, buffer): + buffer.append(lidar_data) + + def process_radar_data(self, radar_data, buffer): + buffer.append(radar_data) + + def save_recordings(self, save_dir): + if not self.record_sensors: + print('No recordings saved; turn on recordings for simulator to enable') + return + + print('Started saving recorded data') + + if not os.path.isdir(save_dir): + os.mkdir(save_dir) + + # Find frame indices for which all sensors have data (so that recordings are synchronized) + common_frame_idxes = None + for sensor in self.sensors: + frame_idxes = {} + + if sensor['type'] == 'rgb': + frame_idxes = {data.frame for data in sensor['rgb_buffer']} + frame_idxes = frame_idxes.intersection({data.frame for data in sensor['depth_buffer']}) + frame_idxes = frame_idxes.intersection({data.frame for data in sensor['semantic_buffer']}) + elif sensor['type'] == 'lidar': + frame_idxes = {data.frame for data in sensor['lidar_buffer']} + elif sensor['type'] == 'radar': + frame_idxes = {data.frame for data in sensor['radar_buffer']} + + if common_frame_idxes is None: + common_frame_idxes = frame_idxes + else: + common_frame_idxes = common_frame_idxes.intersection(frame_idxes) + + # Intersect with bounding box frame indices + common_frame_idxes = common_frame_idxes.intersection({frame_idx for frame_idx, _ in self.bbox_buffer}) + + common_frame_idxes = sorted(list(common_frame_idxes)) + + for sensor in self.sensors: + if sensor['type'] == 'rgb': + rgb_recording = rec_utils.VideoRecording() + depth_recording = rec_utils.VideoRecording() + semantic_recording = rec_utils.VideoRecording() + + rgb_data = {data.frame: data for data in sensor['rgb_buffer']} + depth_data = {data.frame: data for data in sensor['depth_buffer']} + semantic_data = {data.frame: data for data in sensor['semantic_buffer']} + + for frame_idx in common_frame_idxes: + rgb_recording.add_frame(rgb_data[frame_idx]) + depth_recording.add_frame(depth_data[frame_idx]) + semantic_recording.add_frame(semantic_data[frame_idx]) + + sensor_dir = os.path.join(save_dir, sensor['name']) + if not os.path.isdir(sensor_dir): + os.mkdir(sensor_dir) + + rgb_filepath = os.path.join(sensor_dir, 'rgb.mp4') + depth_filepath = os.path.join(sensor_dir, 'depth.mp4') + semantic_filepath = os.path.join(sensor_dir, 'semantic.mp4') + + rgb_recording.save(rgb_filepath) + depth_recording.save(depth_filepath) + semantic_recording.save(semantic_filepath) + + elif sensor['type'] == 'lidar': + lidar_recording = rec_utils.FrameRecording() + + lidar_data = {data.frame: data for data in sensor['lidar_buffer']} + + for frame_idx in common_frame_idxes: + classified_lidar_points = [[i.point.x, i.point.y, i.point.z, i.object_tag] for i in lidar_data[frame_idx]] + lidar_recording.add_frame(classified_lidar_points) + + sensor_dir = os.path.join(save_dir, sensor['name']) + if not os.path.isdir(sensor_dir): + os.mkdir(sensor_dir) + + lidar_filepath = os.path.join(sensor_dir, 'lidar.json') + + lidar_recording.save(lidar_filepath) + + elif sensor['type'] == 'radar': + radar_recording = rec_utils.FrameRecording() + + radar_data = {data.frame: data for data in sensor['radar_buffer']} + + + # Save bounding boxes + bbox_dir = os.path.join(save_dir, 'annotations') + if not os.path.isdir(bbox_dir): + os.mkdir(bbox_dir) + + bbox_recording = rec_utils.BBoxRecording() + + bbox_data = {frame_idx: bboxes for frame_idx, bboxes in self.bbox_buffer} + + for frame_idx in common_frame_idxes: + bbox_recording.add_frame(bbox_data[frame_idx]) + + bbox_filepath = os.path.join(bbox_dir, 'bboxes.json') + bbox_recording.save(bbox_filepath) \ No newline at end of file diff --git a/src/scenic/simulators/carla/utils/recording_utils.py b/src/scenic/simulators/carla/utils/recording_utils.py index 91ea1512d..fbb573328 100644 --- a/src/scenic/simulators/carla/utils/recording_utils.py +++ b/src/scenic/simulators/carla/utils/recording_utils.py @@ -7,9 +7,11 @@ import carla +import copy import cv2 import json import numpy as np +import os try: import pygame @@ -22,12 +24,6 @@ except ImportError: pass -VIEW_WIDTH = 1280.0 -VIEW_HEIGHT = 720.0 -VIEW_FOV = 90.0 - -BB_COLOR = (248, 64, 24) - class BBoxUtil(object): """ This is a module responsible for creating 3D bounding boxes and drawing them @@ -35,7 +31,7 @@ class BBoxUtil(object): """ @staticmethod - def get_bounding_boxes(vehicles, camera): + def get_3d_bounding_boxes_projected(vehicles, camera): """ Creates 3D bounding boxes based on carla vehicle list and camera. """ @@ -46,18 +42,43 @@ def get_bounding_boxes(vehicles, camera): return bounding_boxes @staticmethod - def get_3d_bounding_boxes(vehicles, camera): + def get_3d_bounding_boxes(vehicles, ego): + """ + Creates 3D bounding boxes of vehicles relative to ego. + """ + bounding_boxes = [] for vehicle in vehicles: bb_cords = BBoxUtil._create_bb_points(vehicle) - cords_x_y_z = BBoxUtil._vehicle_to_sensor(bb_cords, vehicle, camera)[:3, :] - cords_y_minus_z_x = np.concatenate([cords_x_y_z[1, :], -cords_x_y_z[2, :], cords_x_y_z[0, :]]) - bbox = np.transpose(np.dot(camera.calibration, cords_y_minus_z_x)) + bbox = BBoxUtil._vehicle_to_ego(bb_cords, vehicle, ego)[:3, :] + bbox = np.transpose(bbox) + bounding_boxes.append(bbox) return bounding_boxes + @staticmethod + def _vehicle_to_ego(cords, vehicle, ego): + """ + Transforms coordinates of a vehicle bounding box to ego. + """ + + world_cord = BBoxUtil._vehicle_to_world(cords, vehicle) + ego_cord = BBoxUtil._world_to_ego(world_cord, ego) + return ego_cord + + @staticmethod + def _world_to_ego(cords, ego): + """ + Transforms world coordinates to ego. + """ + + ego_world_matrix = BBoxUtil.get_matrix(ego.carlaActor.get_transform()) + world_ego_matrix = np.linalg.inv(ego_world_matrix) + ego_cords = np.dot(world_ego_matrix, cords) + return ego_cords + @staticmethod def get_2d_bounding_boxes(bounding_boxes_3d): min_x_coords = [np.min(bbox[:, 0]) for bbox in bounding_boxes_3d] @@ -85,6 +106,11 @@ def draw_bounding_boxes(display, bounding_boxes): Draws bounding boxes on pygame display. """ + VIEW_WIDTH = 1280.0 + VIEW_HEIGHT = 720.0 + + BB_COLOR = (248, 64, 24) + bb_surface = pygame.Surface((VIEW_WIDTH, VIEW_HEIGHT)) bb_surface.set_colorkey((0, 0, 0)) for bbox in bounding_boxes: @@ -241,7 +267,6 @@ def add_frame(self, bounding_boxes): def get_frame(self, frame_idx): return self.frames[frame_idx] - @staticmethod def import_from_file(filepath): with open(filepath, 'r') as f: json_data = json.load(f) @@ -268,7 +293,6 @@ def add_frame(self, frame_data): def get_frame(self, frame_idx): return self.frames[frame_idx] - @staticmethod def import_from_file(filepath): stream = cv2.VideoCapture(filepath) num_frames = int(stream.get(cv2.CAP_PROP_FRAME_COUNT)) @@ -292,7 +316,7 @@ def save(self, filepath): out = cv2.VideoWriter( filepath, cv2.VideoWriter_fourcc(*'mp4v'), - 30.0, + 15.0, (frame_width, frame_height), True ) @@ -302,9 +326,9 @@ def save(self, filepath): out.release() -class LidarRecording: +class FrameRecording: def __init__(self, frames=None): - # Each frame is a list of classified lidar points + # Each frame is a list of data, be it classified lidar points, radar, etc. self.frames = [] if frames is not None: self.frames = frames @@ -315,12 +339,11 @@ def add_frame(self, lidar_points): def get_frame(self, frame_idx): return self.frames[frame_idx] - @staticmethod def import_from_file(filepath): with open(filepath, 'r') as f: json_data = json.load(f) - return LidarRecording(json_data) + return FrameRecording(json_data) def save(self, filepath): with open(filepath, 'w') as f: