dataset_v1.py

#!/usr/bin/env python

# Copyright (c) 2019 Aptiv
#
# This work is licensed under the terms of the MIT license.
# For a copy, see <https://opensource.org/licenses/MIT>.

"""
An example of client-side bounding boxes with basic car controls.

Controls:

    W            : throttle
    S            : brake
    AD           : steer
    Space        : hand-brake

    ESC          : quit
"""

# ==============================================================================
# -- find carla module ---------------------------------------------------------
# ==============================================================================



import glob
import numpy
import os
import sys
import json
import math
import mathutils
import weakref


try:
    sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % (
        sys.version_info.major,
        sys.version_info.minor,
        'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
except IndexError:
    pass


# ==============================================================================
# -- imports -------------------------------------------------------------------
# ==============================================================================

import carla
import random



try:
    import pygame
    from pygame.locals import K_ESCAPE
    from pygame.locals import K_SPACE
    from pygame.locals import K_a
    from pygame.locals import K_d
    from pygame.locals import K_s
    from pygame.locals import K_w
except ImportError:
    raise RuntimeError('cannot import pygame, make sure pygame package is installed')

try:
    import numpy as np
except ImportError:
    raise RuntimeError('cannot import numpy, make sure numpy package is installed')

VIEW_WIDTH = 640
VIEW_HEIGHT = 480
VIEW_FOV = 90

BB_COLOR = (248, 64, 24)


    
def appendSpherical_np(xyz):
    ptsnew = np.hstack((xyz, np.zeros(xyz.shape)))
    xy = xyz[:,0]**2 + xyz[:,1]**2
    ptsnew[:,3] = np.sqrt(xy + xyz[:,2]**2)
    ptsnew[:,4] = np.arctan2(np.sqrt(xy), xyz[:,2]) # for elevation angle defined from Z-axis down
    #ptsnew[:,4] = np.arctan2(xyz[:,2], np.sqrt(xy)) # for elevation angle defined from XY-plane up
    ptsnew[:,5] = np.arctan2(xyz[:,1], xyz[:,0])
    return ptsnew

# ==============================================================================
# -- ClientSideBoundingBoxes ---------------------------------------------------
# ==============================================================================


class ClientSideBoundingBoxes(object):
    """
    This is a module responsible for creating 3D bounding boxes and drawing them
    client-side on pygame surface.
    """

    @staticmethod
    def get_bounding_boxes(vehicles, camera):
        """
        Creates 3D bounding boxes based on carla vehicle list and camera.
        """

        bounding_boxes = [ClientSideBoundingBoxes.get_bounding_box(vehicle, camera) for vehicle in vehicles]
        # filter objects behind camera
        bounding_boxes = [bb for bb in bounding_boxes if all(bb[:, 2] > 0)]
        return bounding_boxes

    @staticmethod
    def draw_bounding_boxes(display, bounding_boxes):
        """
        Draws bounding boxes on pygame display.
        """

        bb_surface = pygame.Surface((VIEW_WIDTH, VIEW_HEIGHT))
        bb_surface.set_colorkey((0, 0, 0))
        for bbox in bounding_boxes:
            points = [(int(bbox[i, 0]), int(bbox[i, 1])) for i in range(8)]
            # draw lines
            # base
            pygame.draw.line(bb_surface, BB_COLOR, points[0], points[1])
            pygame.draw.line(bb_surface, BB_COLOR, points[0], points[1])
            pygame.draw.line(bb_surface, BB_COLOR, points[1], points[2])
            pygame.draw.line(bb_surface, BB_COLOR, points[2], points[3])
            pygame.draw.line(bb_surface, BB_COLOR, points[3], points[0])
            # top
            pygame.draw.line(bb_surface, BB_COLOR, points[4], points[5])
            pygame.draw.line(bb_surface, BB_COLOR, points[5], points[6])
            pygame.draw.line(bb_surface, BB_COLOR, points[6], points[7])
            pygame.draw.line(bb_surface, BB_COLOR, points[7], points[4])
            # base-top
            pygame.draw.line(bb_surface, BB_COLOR, points[0], points[4])
            pygame.draw.line(bb_surface, BB_COLOR, points[1], points[5])
            pygame.draw.line(bb_surface, BB_COLOR, points[2], points[6])
            pygame.draw.line(bb_surface, BB_COLOR, points[3], points[7])
        display.blit(bb_surface, (0, 0))

    @staticmethod
    def get_bounding_box(vehicle, camera):
        """
        Returns 3D bounding box for a vehicle based on camera view.
        """

        bb_cords = ClientSideBoundingBoxes._create_bb_points(vehicle)
        cords_x_y_z = ClientSideBoundingBoxes._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))
        camera_bbox = np.concatenate([bbox[:, 0] / bbox[:, 2], bbox[:, 1] / bbox[:, 2], bbox[:, 2]], axis=1)
        return camera_bbox

    @staticmethod
    def _create_bb_points(vehicle):
        """
        Returns 3D bounding box for a vehicle.
        """

        cords = np.zeros((8, 4))
        extent = vehicle.bounding_box.extent
        cords[0, :] = np.array([extent.x, extent.y, -extent.z, 1])
        cords[1, :] = np.array([-extent.x, extent.y, -extent.z, 1])
        cords[2, :] = np.array([-extent.x, -extent.y, -extent.z, 1])
        cords[3, :] = np.array([extent.x, -extent.y, -extent.z, 1])
        cords[4, :] = np.array([extent.x, extent.y, extent.z, 1])
        cords[5, :] = np.array([-extent.x, extent.y, extent.z, 1])
        cords[6, :] = np.array([-extent.x, -extent.y, extent.z, 1])
        cords[7, :] = np.array([extent.x, -extent.y, extent.z, 1])
        return cords

    @staticmethod
    def _vehicle_to_sensor(cords, vehicle, sensor):
        """
        Transforms coordinates of a vehicle bounding box to sensor.
        """

        world_cord = ClientSideBoundingBoxes._vehicle_to_world(cords, vehicle)
        sensor_cord = ClientSideBoundingBoxes._world_to_sensor(world_cord, sensor)
        return sensor_cord

    @staticmethod
    def _vehicle_to_world(cords, vehicle):
        """
        Transforms coordinates of a vehicle bounding box to world.
        """

        bb_transform = carla.Transform(vehicle.bounding_box.location)
        bb_vehicle_matrix = ClientSideBoundingBoxes.get_matrix(bb_transform)
        vehicle_world_matrix = ClientSideBoundingBoxes.get_matrix(vehicle.get_transform())
        bb_world_matrix = np.dot(vehicle_world_matrix, bb_vehicle_matrix)
        world_cords = np.dot(bb_world_matrix, np.transpose(cords))
        return world_cords

    @staticmethod
    def _world_to_sensor(cords, sensor):
        """
        Transforms world coordinates to sensor.
        """

        sensor_world_matrix = ClientSideBoundingBoxes.get_matrix(sensor.get_transform())
        world_sensor_matrix = np.linalg.inv(sensor_world_matrix)
        sensor_cords = np.dot(world_sensor_matrix, cords)
        return sensor_cords

    @staticmethod
    def get_matrix(transform):
        """
        Creates matrix from carla transform.
        """

        rotation = transform.rotation
        location = transform.location
        c_y = np.cos(np.radians(rotation.yaw))
        s_y = np.sin(np.radians(rotation.yaw))
        c_r = np.cos(np.radians(rotation.roll))
        s_r = np.sin(np.radians(rotation.roll))
        c_p = np.cos(np.radians(rotation.pitch))
        s_p = np.sin(np.radians(rotation.pitch))
        matrix = np.matrix(np.identity(4))
        matrix[0, 3] = location.x
        matrix[1, 3] = location.y
        matrix[2, 3] = location.z
        matrix[0, 0] = c_p * c_y
        matrix[0, 1] = c_y * s_p * s_r - s_y * c_r
        matrix[0, 2] = -c_y * s_p * c_r - s_y * s_r
        matrix[1, 0] = s_y * c_p
        matrix[1, 1] = s_y * s_p * s_r + c_y * c_r
        matrix[1, 2] = -s_y * s_p * c_r + c_y * s_r
        matrix[2, 0] = s_p
        matrix[2, 1] = -c_p * s_r
        matrix[2, 2] = c_p * c_r
        return matrix


# ==============================================================================
# -- BasicSynchronousClient ----------------------------------------------------
# ==============================================================================


class BasicSynchronousClient(object):
    """
    Basic implementation of a synchronous client.
    """

    def __init__(self):
        self.client = None
        self.world = None
        self.camera = None
        self.car = None

        self.display = None
        self.image = None
        self.capture = True

    def camera_depth(self):
        """
        Returns camera blueprint.
        """

        camera_dp = self.world.get_blueprint_library().find('sensor.camera.depth')
        camera_dp.set_attribute('image_size_x', str(VIEW_WIDTH))
        camera_dp.set_attribute('image_size_y', str(VIEW_HEIGHT))
        camera_dp.set_attribute('fov', str(VIEW_FOV))
        return camera_dp        

    def camera_blueprint(self):
        """
        Returns camera blueprint.
        """

        camera_bp = self.world.get_blueprint_library().find('sensor.camera.rgb')
        camera_bp.set_attribute('image_size_x', str(VIEW_WIDTH))
        camera_bp.set_attribute('image_size_y', str(VIEW_HEIGHT))
        camera_bp.set_attribute('fov', str(VIEW_FOV))
        return camera_bp

    def set_synchronous_mode(self, synchronous_mode):
        """
        Sets synchronous mode.
        """

        settings = self.world.get_settings()
        settings.synchronous_mode = synchronous_mode
        self.world.apply_settings(settings)

    def setup_car(self):
        """
        Spawns actor-vehicle to be controled.
        """

        car_bp = self.world.get_blueprint_library().filter('vehicle*')[31]
        
        spawn_point=self.world.get_map().get_spawn_points()[230]
     

        
        self.car = self.world.spawn_actor(car_bp, spawn_point)

    def setup_camera(self,img_num,init_x,init_y,deg,z):
        """
        Spawns actor-camera to be used to render view.
        Sets calibration for client-side boxes rendering.
        """        
        v=mathutils.Vector((init_x, init_y,z))
        
        camera_transform = carla.Transform(carla.Location(v[0],v[1] , v[2]), carla.Rotation(pitch=180,yaw=deg,roll=180))
        tn=camera_transform.get_matrix()
        print('part of rotation matrix')
        print(tn[1:2])
        kp=numpy.squeeze(np.asarray(tn))
        ligne1=str(kp[0:1])[2:-2]
        ligne2=str(kp[1:2])[2:-2]
        ligne3=str(kp[2:3])[2:-2]
        
        print("location is ",camera_transform)
        ppt=str(camera_transform)
        pos1 = ppt.find('pitch=')
        pos2 = ppt.find('))')
        
        sp = ppt[pos1:pos2]
        pos3=sp.find('pitch=')
        pos4 = sp.find(',')
        sp2=sp[pos3:pos4]
        l,d=sp2.split('=')
        print("show",d)
        pos5=sp.find('yaw=')
        pos6 = sp.find('))')
        sp3=sp[pos5:pos6]
        pos7=sp3.find('yaw=')
        pos8=sp3.find(',')
        sp4=sp3[pos7:pos8]
        j,k=sp4.split('=')
        print("yaw = ",k)
        pos9=sp3.find('roll=')
        pos10 = sp3.find('))')
        sp5=sp3[pos9:pos10]
        h,p=sp5.split('=')
        print(p)
        self.camera = self.world.spawn_actor(self.camera_blueprint(), camera_transform, attach_to=self.car)


        weak_self = weakref.ref(self)
        self.camera.listen(lambda image: weak_self().set_image(weak_self, image))
        calibration = np.identity(3)
        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))
        self.camera.calibration = calibration

        file = os.path.join("D:\WindowsNoEditor\PythonAPI\examples/new", 'IMG-' + str(img_num) )
        f = open(file+'.txt', "a")
        f.write(ligne1)
        f.write("\n")
        f.write(ligne2)
        f.write("\n")
        f.write(ligne3)
        f.write("\n")
        f.write(' 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00')

        f.close()





    def setup_cam(self,img_num,init_x,init_y,deg,z):
        """
        Spawns actor-camera to be used to render view.
        Sets calibration for client-side boxes rendering.
        """        
        v=mathutils.Vector((init_x, init_y,z))
        
        camera_transform = carla.Transform(carla.Location(v[0],v[1] , v[2]), carla.Rotation(pitch=180,yaw=deg,roll=180))
        tn=camera_transform.get_matrix()
        print(tn)
        print("location is ",camera_transform)
        ppt=str(camera_transform)
        pos1 = ppt.find('pitch=')
        pos2 = ppt.find('))')
        
        sp = ppt[pos1:pos2]
        pos3=sp.find('pitch=')
        pos4 = sp.find(',')
        sp2=sp[pos3:pos4]
        l,d=sp2.split('=')
        print("show",d)
        pos5=sp.find('yaw=')
        pos6 = sp.find('))')
        sp3=sp[pos5:pos6]
        pos7=sp3.find('yaw=')
        pos8=sp3.find(',')
        sp4=sp3[pos7:pos8]
        j,k=sp4.split('=')
        print("yaw = ",k)
        pos9=sp3.find('roll=')
        pos10 = sp3.find('))')
        sp5=sp3[pos9:pos10]
        h,p=sp5.split('=')
        print(p)
        self.camera = self.world.spawn_actor(self.camera_depth(), camera_transform, attach_to=self.car)


        weak_self = weakref.ref(self)
        self.camera.listen(lambda image: weak_self().set_image(weak_self, image))
        calibration = np.identity(3)
        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))
        self.camera.calibration = calibration











    def control(self, car):
        """
        Applies control to main car based on pygame pressed keys.
        Will return True If ESCAPE is hit, otherwise False to end main loop.
        """

        keys = pygame.key.get_pressed()
        if keys[K_ESCAPE]:
            return True

        control = car.get_control()
        control.throttle = 0
        if keys[K_w]:
            control.throttle = 1
            control.reverse = False
        elif keys[K_s]:
            control.throttle = 1
            control.reverse = True
        if keys[K_a]:
            control.steer = max(-1., min(control.steer - 0.05, 0))
        elif keys[K_d]:
            control.steer = min(1., max(control.steer + 0.05, 0))
        else:
            control.steer = 0
        control.hand_brake = keys[K_SPACE]

        car.apply_control(control)
        return False

    @staticmethod
    def set_image(weak_self, img):
        """
        Sets image coming from camera sensor.
        The self.capture flag is a mean of synchronization - once the flag is
        set, next coming image will be stored.
        """

        self = weak_self()
        if self.capture:
            self.image = img
            self.capture = False

    def render(self, display,img_num):
        """
        Transforms image from camera sensor and blits it to main pygame display.
        """

        if self.image is not None:
            array = np.frombuffer(self.image.raw_data, dtype=np.dtype("uint8"))
            array = np.reshape(array, (self.image.height, self.image.width, 4))
            array = array[:, :, :3]
            array = array[:, :, ::-1]
            jik='new/IMG-'+str(img_num)+'.png'       
            self.image.save_to_disk(jik)
           

    def rend(self, display,img_num):
        """
        Transforms image from camera sensor and blits it to main pygame display.
        """

        if self.image is not None:
            self.image.convert(carla.ColorConverter.LogarithmicDepth)
            array = np.frombuffer(self.image.raw_data, dtype=np.dtype("uint8"))
            array = np.reshape(array, (self.image.height, self.image.width, 4))
            array = array[:, :, :3]
            array = array[:, :, ::-1]
            jik='new/IMG_depth-'+str(img_num)+'.png'             
            #self.image.save_to_disk(jik,self.camera.Depth)
            self.image.save_to_disk(jik)
                              
            

            surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
            display.blit(surface, (0, 0))
           

    def game_loop(self):
        """
        Main program loop.
        """

        try:
            pygame.init()

            self.client = carla.Client('127.0.0.1', 2000)
            self.client.set_timeout(2.0)
            self.world = self.client.get_world()

            self.setup_car()
           

            self.display = pygame.display.set_mode((VIEW_WIDTH, VIEW_HEIGHT), pygame.HWSURFACE | pygame.DOUBLEBUF)
            pygame_clock = pygame.time.Clock()

            self.set_synchronous_mode(True)
            vehicles = self.world.get_actors().filter('vehicle.*')
            
           
            used=[]
            img_num = 2304


          
            used_randoms=[]
            for i in range (4):



                #radius=6-i
                

                for degrees in range(0,360, 15):
                    z=random.uniform(0.5,1.5)
                    while z in used:
                        z = random.uniform(0.5,1.5)
                    used.append(z)

                    radius=random.uniform(2.75, 3.75)
                    while radius in used_randoms:
                        radius = random.uniform(2.75, 3.75)
                    used_randoms.append(radius)

                    radians = math.radians(degrees)
                
                    x= math.cos(radians) * radius
                    y= math.sin(radians) * radius
                    print("for angle=",degrees,"x=",x,"y=",y)
           
                
                    img_num += 1
                    
                 
                    
               
                    self.setup_camera(img_num,x,y,degrees,z)
                
                    tt=0
                
                    while (tt<7):
                    

                        self.world.tick()

                        self.capture = True
                        pygame_clock.tick_busy_loop(20)

                        self.render(self.display,img_num)
                        bounding_boxes = ClientSideBoundingBoxes.get_bounding_boxes(vehicles, self.camera)

                        pygame.display.update()

                        pygame.event.pump()
                        if self.control(self.car):
                            return
                        tt+=1
         
                    self.setup_cam(img_num,x,y,degrees,z)
                    tt=0
                
                    while (tt<7):
                    

                        self.world.tick()

                        self.capture = True
                        pygame_clock.tick_busy_loop(20)

                        self.rend(self.display,img_num)
                        bounding_boxes = ClientSideBoundingBoxes.get_bounding_boxes(vehicles, self.camera)

                        pygame.display.update()

                        pygame.event.pump()
                        if self.control(self.car):
                            return
                        tt+=1
                         

            
                
                

        finally:
            
            self.set_synchronous_mode(False)
            self.camera.destroy()
            self.car.destroy()
            pygame.quit()


# ==============================================================================
# -- main() --------------------------------------------------------------------
# ==============================================================================


def main():
    """
    Initializes the client-side bounding box demo.
    """

    try:
        client = BasicSynchronousClient()
        client.game_loop()
        
    finally:
        print('EXIT')


if __name__ == '__main__':
    main()