client_example_matrix.py

#!/usr/bin/env python3

# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de
# Barcelona (UAB).
#
# This work is licensed under the terms of the MIT license.
# For a copy, see <https://opensource.org/licenses/MIT>.

"""Basic CARLA client example."""

from __future__ import print_function

import argparse
import logging
import random
import time
import os
import numpy as np

from carla.client import make_carla_client
from carla.sensor import Camera, Lidar
from carla.settings import CarlaSettings
from carla.tcp import TCPConnectionError
from carla.util import print_over_same_line
from carla.transform import Transform #, Translation, Rotation, Scale
from carla import image_converter
import cv2


def run_carla_client(args):
    # Here we will run 3 episodes with 300 frames each.
    number_of_episodes = 15
    frames_per_episode = 1230
    #              [0 , 1 , 2 , 3 , 4  , 5  , 6  , 7  , 8  , 9  , 10, 11, 12, 13, 14]
    vehicles_num = [140, 100, 120, 80, 90, 100, 80, 90, 110, 100, 80, 70, 70, 80, 100]

    # We assume the CARLA server is already waiting for a client to connect at
    # host:port. To create a connection we can use the `make_carla_client`
    # context manager, it creates a CARLA client object and starts the
    # connection. It will throw an exception if something goes wrong. The
    # context manager makes sure the connection is always cleaned up on exit.
    with make_carla_client(args.host, args.port) as client:
        print('CarlaClient connected')

        for episode in range(0, number_of_episodes):
            # Start a new episode.

            if args.settings_filepath is None:

                # Create a CarlaSettings object. This object is a wrapper around
                # the CarlaSettings.ini file. Here we set the configuration we
                # want for the new episode.
                settings = CarlaSettings()
                settings.set(
                    SynchronousMode=True,
                    SendNonPlayerAgentsInfo=False,
                    NumberOfVehicles= vehicles_num[episode],#random.choice([0, 20, 15, 20, 25, 21, 24, 18, 40, 35, 25, 30]), #25,
                    NumberOfPedestrians=120,
                    DisableTwoWheeledVehicles=False,
                    WeatherId= episode, #1, #random.choice([1, 3, 7, 8, 14]),
                    QualityLevel=args.quality_level)
                settings.randomize_seeds()

                # Now we want to add a couple of cameras to the player vehicle.
                # We will collect the images produced by these cameras every
                # frame.

                # LEFT RGB CAMERA
                camera_l = Camera('LeftCameraRGB', PostProcessing='SceneFinal')
                camera_l.set_image_size(800, 600)
                camera_l.set_position(1.30, -0.27, 1.50)
                settings.add_sensor(camera_l)

                # LEFT DEPTH
                camera_ld = Camera('LeftCameraDepth', PostProcessing='Depth')
                camera_ld.set_image_size(800, 600)
                camera_ld.set_position(1.30, -0.27, 1.50)
                settings.add_sensor(camera_ld)

                # LEFT SEGMENTATION
                camera_ls = Camera('LeftCameraSeg', PostProcessing='SemanticSegmentation')
                camera_ls.set_image_size(800, 600)
                camera_ls.set_position(1.30, -0.27, 1.50)
                settings.add_sensor(camera_ls)

                # RIGHT RGB CAMERA
                camera_r = Camera('RightCameraRGB', PostProcessing='SceneFinal')
                camera_r.set_image_size(800, 600)
                camera_r.set_position(1.30, 0.27, 1.50)
                settings.add_sensor(camera_r)

                # RIGHT DEPTH
                camera_rd = Camera('RightCameraDepth', PostProcessing='Depth')
                camera_rd.set_image_size(800, 600)
                camera_rd.set_position(1.30, 0.27, 1.50)
                settings.add_sensor(camera_rd)

                # RIGHT SEGMENTATION
                camera_rs = Camera('RightCameraSeg', PostProcessing='SemanticSegmentation')
                camera_rs.set_image_size(800, 600)
                camera_rs.set_position(1.30, 0.27, 1.50)
                settings.add_sensor(camera_rs)

                # LEFT 15 DEGREE RGB CAMERA
                camera_l_15 = Camera('15_LeftCameraRGB', PostProcessing='SceneFinal')
                camera_l_15.set_image_size(800, 600)
                camera_l_15.set_position(1.30, -0.7, 1.50) # [X, Y, Z]
                camera_l_15.set_rotation(0, -15.0, 0)      # [pitch(Y), yaw(Z), roll(X)]
                settings.add_sensor(camera_l_15)

                # LEFT 15 DEGREE DEPTH
                camera_ld_15 = Camera('15_LeftCameraDepth', PostProcessing='Depth')
                camera_ld_15.set_image_size(800, 600)
                camera_ld_15.set_position(1.30, -0.7, 1.50)
                camera_ld_15.set_rotation(0, -15.0, 0)
                settings.add_sensor(camera_ld_15)

                # LEFT 15 DEGREE SEGMENTATION
                camera_ls_15 = Camera('15_LeftCameraSeg', PostProcessing='SemanticSegmentation')
                camera_ls_15.set_image_size(800, 600)
                camera_ls_15.set_position(1.30, -0.7, 1.50)
                camera_ls_15.set_rotation(0, -15.0, 0)
                settings.add_sensor(camera_ls_15)

                # Right 15 DEGREE RGB CAMERA
                camera_r_15 = Camera('15_RightCameraRGB', PostProcessing='SceneFinal')
                camera_r_15.set_image_size(800, 600)
                camera_r_15.set_position(1.30, 0.7, 1.50)
                camera_r_15.set_rotation(0, 15.0, 0)
                settings.add_sensor(camera_r_15)

                # Right 15 DEGREE DEPTH
                camera_rd_15 = Camera('15_RightCameraDepth', PostProcessing='Depth')
                camera_rd_15.set_image_size(800, 600)
                camera_rd_15.set_position(1.30, 0.7, 1.50)
                camera_rd_15.set_rotation(0, 15.0, 0)
                settings.add_sensor(camera_rd_15)

                # RIGHT 15 DEGREE SEGMENTATION
                camera_ls_15 = Camera('15_RightCameraSeg', PostProcessing='SemanticSegmentation')
                camera_ls_15.set_image_size(800, 600)
                camera_ls_15.set_position(1.30, 0.7, 1.50)
                camera_ls_15.set_rotation(0, 15.0, 0)
                settings.add_sensor(camera_ls_15)

                # LEFT 30 DEGREE RGB CAMERA
                camera_l_30 = Camera('30_LeftCameraRGB', PostProcessing='SceneFinal')
                camera_l_30.set_image_size(800, 600)
                camera_l_30.set_position(1.30, -0.7, 1.50)
                camera_l_30.set_rotation(0, -30.0, 0)
                settings.add_sensor(camera_l_30)

                # LEFT 30 DEGREE DEPTH
                camera_ld_30 = Camera('30_LeftCameraDepth', PostProcessing='Depth')
                camera_ld_30.set_image_size(800, 600)
                camera_ld_30.set_position(1.30, -0.7, 1.50)
                camera_ld_30.set_rotation(0, -30.0, 0)
                settings.add_sensor(camera_ld_30)

                # LEFT 30 DEGREE SEGMENTATION
                camera_ls_30 = Camera('30_LeftCameraSeg', PostProcessing='SemanticSegmentation')
                camera_ls_30.set_image_size(800, 600)
                camera_ls_30.set_position(1.30, -0.7, 1.50)
                camera_ls_30.set_rotation(0, -30.0, 0)
                settings.add_sensor(camera_ls_30)

                # RIGHT 30 DEGREE RGB CAMERA
                camera_r_30 = Camera('30_RightCameraRGB', PostProcessing='SceneFinal')
                camera_r_30.set_image_size(800, 600)
                camera_r_30.set_position(1.30, 0.7, 1.50)
                camera_r_30.set_rotation(0, 30.0, 0)
                settings.add_sensor(camera_r_30)

                # RIGHT 30 DEGREE DEPTH
                camera_rd_30 = Camera('30_RightCameraDepth', PostProcessing='Depth')
                camera_rd_30.set_image_size(800, 600)
                camera_rd_30.set_position(1.30, 0.7, 1.50)
                camera_rd_30.set_rotation(0, 30.0, 0)
                settings.add_sensor(camera_rd_30)

                # RIGHT 30 DEGREE SEGMENTATION
                camera_rs_30 = Camera('30_RightCameraSeg', PostProcessing='SemanticSegmentation')
                camera_rs_30.set_image_size(800, 600)
                camera_rs_30.set_position(1.30, 0.7, 1.50)
                camera_rs_30.set_rotation(0, 30.0, 0)
                settings.add_sensor(camera_rs_30)
            else:
                # Alternatively, we can load these settings from a file.
                with open(args.settings_filepath, 'r') as fp:
                    settings = fp.read()

            # Now we load these settings into the server. The server replies
            # with a scene description containing the available start spots for
            # the player. Here we can provide a CarlaSettings object or a
            # CarlaSettings.ini file as string.
            scene = client.load_settings(settings)

            # Choose one player start at random.
            number_of_player_starts = len(scene.player_start_spots)
            player_start = random.randint(0, max(0, number_of_player_starts - 1))

            # Notify the server that we want to start the episode at the
            # player_start index. This function blocks until the server is ready
            # to start the episode.
            print('Starting new episode...')
            client.start_episode(player_start)

            camera_l_to_car_transform    = camera_l.get_unreal_transform()
            camera_r_to_car_transform    = camera_r.get_unreal_transform()
            camera_l_15_to_car_transform = camera_l_15.get_unreal_transform()
            camera_r_15_to_car_transform = camera_r_15.get_unreal_transform()
            camera_l_30_to_car_transform = camera_l_30.get_unreal_transform()
            camera_r_30_to_car_transform = camera_r_30.get_unreal_transform()

            if not os.path.isdir(args.dataset_path + "/episode_{:0>4d}".format(episode)):
                os.makedirs(args.dataset_path + "/episode_{:0>4d}".format(episode))
            # Iterate every frame in the episode.
            for frame in range(0, frames_per_episode):

                # Read the data produced by the server this frame.
                measurements, sensor_data = client.read_data()

                #image = sensor_data.get('LeftCameraSeg', None)
                # array = image_converter.depth_to_logarithmic_grayscale(image)
                #array = image_converter.labels_to_cityscapes_palette(image)
                #filename = '{:0>6d}'.format(frame)
                #filename += '.png'
                #cv2.imwrite(filename, array)
                #image.save_to_disk("/data/khoshhal/Dataset/episode_{:0>4d}/{:s}/{:0>6d}".format(episode, "LeftCameraSeg", frame))

                # Print some of the measurements.
                # print_measurements(measurements)

                #player_measurements = measurements.player_measurements
                world_transform = Transform(measurements.player_measurements.transform)

                # Compute the final transformation matrix.
                camera_l_to_world_transform = world_transform * camera_l_to_car_transform
                camera_r_to_world_transform = world_transform * camera_r_to_car_transform
                camera_l_15_to_world_transform = world_transform * camera_l_15_to_car_transform
                camera_r_15_to_world_transform = world_transform * camera_r_15_to_car_transform
                camera_l_30_to_world_transform = world_transform * camera_l_30_to_car_transform
                camera_r_30_to_world_transform = world_transform * camera_r_30_to_car_transform
                args.out_filename_format = dataset_path + '/episode_{:0>4d}/{:s}/{:0>6d}'
                # Save the images to disk if requested.
                if frame >= 30:
                    if args.save_images_to_disk:
                        for name, measurement in sensor_data.items():
                            filename = args.out_filename_format.format(episode, name, frame-30)
                            measurement.save_to_disk(filename)

                        # Save Transform matrix of each camera to separated files
                        line = ""
                        filename = "{}/episode_{:0>4d}/LeftCamera".format(args.dataset_path, episode) + ".txt"
                        with open(filename, 'a') as LeftCamera:
                            for x in np.asarray(camera_l_to_world_transform.matrix[:3, :]).reshape(-1):
                                line += "{:.8e} ".format(x)
                            line = line[:-1]
                            line += "\n"
                            LeftCamera.write(line)
                            line = ""
                        filename = "{}/episode_{:0>4d}/RightCamera".format(args.dataset_path, episode) + ".txt"
                        with open(filename, 'a') as RightCamera:
                            for x in np.asarray(camera_r_to_world_transform.matrix[:3, :]).reshape(-1):
                                line += "{:.8e} ".format(x)
                            line = line[:-1]
                            line += "\n"
                            RightCamera.write(line)
                            line = ""

                        filename = "{}/episode_{:0>4d}/15_LeftCamera".format(args.dataset_path, episode) + ".txt"
                        with open(filename, 'a') as myfile:
                            for x in np.asarray(camera_l_15_to_world_transform.matrix[:3, :]).reshape(-1):
                                line += "{:.8e} ".format(x)
                            line = line[:-1]
                            line += "\n"
                            myfile.write(line)
                            line = ""

                        filename = "{}/episode_{:0>4d}/15_RightCamera".format(args.dataset_path, episode) + ".txt"
                        with open(filename, 'a') as myfile:
                            for x in np.asarray(camera_r_15_to_world_transform.matrix[:3, :]).reshape(-1):
                                line += "{:.8e} ".format(x)
                            line = line[:-1]
                            line += "\n"
                            myfile.write(line)
                            line = ""

                        filename = "{}/episode_{:0>4d}/30_LeftCamera".format(args.dataset_path, episode) + ".txt"
                        with open(filename, 'a') as myfile:
                            for x in np.asarray(camera_l_30_to_world_transform.matrix[:3, :]).reshape(-1):
                                line += "{:.8e} ".format(x)
                            line = line[:-1]
                            line += "\n"
                            myfile.write(line)
                            line = ""

                        filename = "{}/episode_{:0>4d}/30_RightCamera".format(args.dataset_path, episode) + ".txt"
                        with open(filename, 'a') as myfile:
                            for x in np.asarray(camera_r_30_to_world_transform.matrix[:3, :]).reshape(-1):
                                line += "{:.8e} ".format(x)
                            line = line[:-1]
                            line += "\n"
                            myfile.write(line)
                            line = ""

                # We can access the encoded data of a given image as numpy
                # array using its "data" property. For instance, to get the
                # depth value (normalized) at pixel X, Y
                #
                #     depth_array = sensor_data['CameraDepth'].data
                #     value_at_pixel = depth_array[Y, X]
                #

                # Now we have to send the instructions to control the vehicle.
                # If we are in synchronous mode the server will pause the
                # simulation until we send this control.

                if not args.autopilot:

                    client.send_control(
                        steer=random.uniform(-1.0, 1.0),
                        throttle=0.5,
                        brake=0.0,
                        hand_brake=False,
                        reverse=False)

                else:

                    # Together with the measurements, the server has sent the
                    # control that the in-game autopilot would do this frame. We
                    # can enable autopilot by sending back this control to the
                    # server. We can modify it if wanted, here for instance we
                    # will add some noise to the steer.

                    control = measurements.player_measurements.autopilot_control
                    #control.steer += random.uniform(-0.1, 0.1)
                    client.send_control(control)

                #time.sleep(1)

            myfile.close()
            LeftCamera.close()
            RightCamera.close()


def print_measurements(measurements):
    number_of_agents = len(measurements.non_player_agents)
    player_measurements = measurements.player_measurements
    message = 'Vehicle at ({pos_x:.1f}, {pos_y:.1f}), '
    message += '{speed:.0f} km/h, '
    message += 'Collision: {{vehicles={col_cars:.0f}, pedestrians={col_ped:.0f}, other={col_other:.0f}}}, '
    message += '{other_lane:.0f}% other lane, {offroad:.0f}% off-road, '
    message += '({agents_num:d} non-player agents in the scene)'
    message = message.format(
        pos_x=player_measurements.transform.location.x,
        pos_y=player_measurements.transform.location.y,
        speed=player_measurements.forward_speed * 3.6, # m/s -> km/h
        col_cars=player_measurements.collision_vehicles,
        col_ped=player_measurements.collision_pedestrians,
        col_other=player_measurements.collision_other,
        other_lane=100 * player_measurements.intersection_otherlane,
        offroad=100 * player_measurements.intersection_offroad,
        agents_num=number_of_agents)
    print_over_same_line(message)


def main():
    argparser = argparse.ArgumentParser(description=__doc__)
    argparser.add_argument(
        '-v', '--verbose',
        action='store_true',
        dest='debug',
        help='print debug information')
    argparser.add_argument(
        '--host',
        metavar='H',
        default='localhost',
        help='IP of the host server (default: localhost)')
    argparser.add_argument(
        '-p', '--port',
        metavar='P',
        default=2000,
        type=int,
        help='TCP port to listen to (default: 2000)')
    argparser.add_argument(
        '-a', '--autopilot',
        action='store_true',
        help='enable autopilot')
    argparser.add_argument(
        '-l', '--lidar',
        action='store_true',
        help='enable Lidar')
    argparser.add_argument(
        '-q', '--quality-level',
        choices=['Low', 'Epic'],
        type=lambda s: s.title(),
        default='Epic',
        help='graphics quality level, a lower level makes the simulation run considerably faster.')
    argparser.add_argument(
        '-i', '--images-to-disk',
        action='store_true',
        dest='save_images_to_disk',
        help='save images (and Lidar data if active) to disk')
    argparser.add_argument(
        '-c', '--carla-settings',
        metavar='PATH',
        dest='settings_filepath',
        default=None,
        help='Path to a "CarlaSettings.ini" file')
    argparser.add_argument(
        '--dataset-path',
        default='/habtegebrialdata/Datasets/carla',
        help='Path to the folder where extracted dataset should be stored ')
    args = argparser.parse_args()

    log_level = logging.DEBUG if args.debug else logging.INFO
    logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level)

    logging.info('listening to server %s:%s', args.host, args.port)

    os.makedirs(args.dataset_path, exist_ok=True)
    #args.dataset_path = '/data/khoshhal/Dataset/'


    while True:
        try:

            run_carla_client(args)

            print('Done.')
            return

        except TCPConnectionError as error:
            logging.error(error)
            time.sleep(1)


if __name__ == '__main__':

    try:
        main()
    except KeyboardInterrupt:
        print('\nCancelled by user. Bye!')