Merge pull request carla-simulator#2 from dianchen96/dev-data_collect

Dev data collect

.gitignore

@@ -8,3 +8,53 @@ LICENSE
 *.txt
 README
 python_api.md
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+
+# C extensions
+*.so
+
+# Distribution / packaging
+bin/
+build/
+develop-eggs/
+dist/
+eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+*.egg-info/
+.installed.cfg
+*.egg
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+.tox/
+.coverage
+.cache
+nosetests.xml
+coverage.xml
+
+# Translations
+*.mo
+
+# Mr Developer
+.mr.developer.cfg
+.project
+.pydevproject
+
+# Rope
+.ropeproject
+
+# Django stuff:
+*.log
+*.pot
+
+# Sphinx documentation
+docs/_build/

CarlaSettings.ini

@@ -0,0 +1,2 @@
+[CARLA/Server]
+DisableRendering=true

PythonAPI/agents/navigation/local_planner.py

@@ -25,6 +25,9 @@ class RoadOption(Enum):
     RIGHT = 2
     STRAIGHT = 3
     LANEFOLLOW = 4
+
+    def __int__(self):
+        return self.value
 
 
 class LocalPlanner(object):
@@ -189,6 +192,7 @@ def run_step(self, debug=True):
         """
 
         # not enough waypoints in the horizon? => add more!
+        # import pdb; pdb.set_trace()
         if len(self._waypoints_queue) < int(self._waypoints_queue.maxlen * 0.5):
             if not self._global_plan:
                 self._compute_next_waypoints(k=100)
@@ -217,6 +221,7 @@ def run_step(self, debug=True):
         # target waypoint
         self._target_waypoint, self._target_road_option = self._waypoint_buffer[0]
         # move using PID controllers
+
         control = self._vehicle_controller.run_step(self._target_speed, self._target_waypoint)
 
         # purge the queue of obsolete waypoints

PythonAPI/agents/navigation/roaming_agent.py

@@ -72,6 +72,6 @@ def run_step(self, debug=False):
         else:
             self._state = AgentState.NAVIGATING
             # standard local planner behavior
-            control = self._local_planner.run_step()
+            control = self._local_planner.run_step(debug=debug)
 
-        return control
+        return control, self._local_planner._target_road_option

bird_view/data_collector.py

@@ -0,0 +1,286 @@
+#!/usr/bin/env python
+
+# 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>.
+
+import glob
+import os
+import sys
+import argparse
+
+try:
+    sys.path.append(glob.glob('../**/*%d.%d-%s.egg' % (
+        sys.version_info.major,
+        sys.version_info.minor,
+        'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
+    sys.path.append(glob.glob('../PythonAPI')[0])
+
+except IndexError:
+    pass
+
+
+
+from time import gmtime, strftime
+
+import carla
+from carla import ColorConverter as cc
+from agents.navigation.roaming_agent import RoamingAgent
+from agents.navigation.basic_agent import BasicAgent
+
+import logging
+import random
+import math
+from skimage.io import imsave
+
+try:
+    import pygame
+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')
+
+try:
+    import queue
+except ImportError:
+    import Queue as queue
+
+from util import *
+from PIL import Image as PImage
+import h5py
+import pathlib
+
+START_OFFSET = 10
+
+def mkdir_p(path):
+    pathlib.Path(path).mkdir(parents=True, exist_ok=True)
+
+
+# def create_hdf5_file(filename, render):
+def to_image(carla_img, color_format):
+    carla_img.convert(color_format)
+    img = np.frombuffer(carla_img.raw_data, dtype=np.dtype("uint8"))
+    img = np.reshape(img, (carla_img.height, carla_img.width, 4))
+    img = img[:, :, :3]
+    img = img[:, :, ::-1]
+
+    return img
+
+
+def main(args):
+
+    client = carla.Client('localhost', 2000)
+    client.set_timeout(10.0)
+
+    world = client.load_world(args.town_name)
+    # world = client.get_world()
+
+    print('enabling synchronous mode.')
+    settings = world.get_settings()
+    settings.synchronous_mode = True
+    world.apply_settings(settings)
+
+    blueprint_library = world.get_blueprint_library()
+
+    vehicle_bp = random.choice(blueprint_library.filter(args.vehicle))
+    vehicle_bp.set_attribute('role_name', 'hero')
+
+    carla_map = world.get_map()
+
+    dataset_prefix = strftime('%Y_%m_%d_%H_%M_%S', gmtime())
+
+    measurements = []
+    bird_view_images = []
+    rgb_images = []
+    depth_images = []
+
+    for traj_num in range(args.num_trajs):
+        # Start a new episode
+
+        dataset_name = os.path.join(args.dataset_path, dataset_prefix, '%03d.hdf5'%traj_num)
+
+        actor_list = []
+
+        # Intialize player
+        start_pose = random.choice(carla_map.get_spawn_points())
+
+        player = world.spawn_actor(vehicle_bp, start_pose)
+        player.set_autopilot(False)
+
+        actor_list.append(player)
+
+        # Setup cameras
+        if args.render:
+            rgb_camera_bp = blueprint_library.find('sensor.camera.rgb')
+            rgb_camera_bp.set_attribute('image_size_x', '400')
+            rgb_camera_bp.set_attribute('image_size_y', '176')
+            rgb_camera_bp.set_attribute('fov', '90')
+            rgb_camera = world.spawn_actor(
+                rgb_camera_bp,
+                carla.Transform(carla.Location(x=2.0, z=1.4), carla.Rotation(pitch=-15)),
+                attach_to=player)
+            actor_list.append(rgb_camera)
+
+            depth_camera_bp = blueprint_library.find('sensor.camera.depth')
+            depth_camera_bp.set_attribute('image_size_x', '400')
+            depth_camera_bp.set_attribute('image_size_y', '176')
+            depth_camera_bp.set_attribute('fov', '90')
+            depth_camera = world.spawn_actor(
+                depth_camera_bp,
+                carla.Transform(carla.Location(x=2.0, z=1.4), carla.Rotation(pitch=-15)),
+                attach_to=player)
+            actor_list.append(depth_camera)
+
+            # Set camera queues
+            rgb_queue = queue.Queue()
+            rgb_camera.listen(rgb_queue.put)
+            depth_queue = queue.Queue()
+            depth_camera.listen(depth_queue.put)
+
+
+        # Spawn cars
+        for _ in range(args.num_vehicles):
+            bp = random.choice(blueprint_library.filter('vehicle.*'))
+            while True:
+                vehicle = world.try_spawn_actor(bp, random.choice(carla_map.get_spawn_points()))
+                if vehicle is not None:
+                    vehicle.set_autopilot(True)
+                    actor_list.append(vehicle)
+                    print ("Spawned vehicle %d"%_)
+                    break
+
+        # Spawn walkers
+        for _ in range(args.num_pedestrians):
+            bp = random.choice(blueprint_library.filter('walker.*'))
+            while True:
+                transform = random.choice(world.get_map().get_spawn_points())
+                # transform = player.get_transform()
+                # transform = start_pose
+                transform.location += carla.Location(x=random.randint(-10,10),y=random.randint(-10,10))
+                walker = world.try_spawn_actor(bp, transform)
+                if walker is not None:
+                    actor_list.append(walker)
+                    print ("Spawned walker %d"%_)
+                    break
+
+        # Setup pygame
+        os.environ["SDL_VIDEODRIVER"] = "dummy"
+        pygame.init()
+        display = pygame.display.set_mode((320, 320),0,32)
+        pygame.display.flip()
+        # Set map drawer module
+        input_module = ModuleInput(MODULE_INPUT)
+        hud_module = ModuleHUD(MODULE_HUD, 320, 320)
+        world_module = ModuleWorld(MODULE_WORLD, client, world, carla_map, player)
+
+        # Register Modules
+        module_manager.register_module(world_module)
+        module_manager.register_module(hud_module)
+        module_manager.register_module(input_module)
+        module_manager.start_modules()
+
+        clock = pygame.time.Clock()
+
+        frame = None
+
+        for t in range(args.episode_length):
+            # clock.tick_busy_loop(1)
+            # module_manager.tick(clock)
+            world.tick()
+            timestamp = world.wait_for_tick()
+            module_manager.tick(clock)
+
+            module_manager.render(display)
+
+            if t == START_OFFSET:
+                agent = RoamingAgent(player)
+
+
+            elif t > START_OFFSET:
+
+                map_image, self_image, vehicle_image, walker_image, traffic_light_image = world_module.get_rendered_surfaces()
+                measurement = world_module.get_hero_meausurements()
+
+                # Save camera image
+                if args.render:
+                    rgb_image = rgb_queue.get()
+                    depth_image = depth_queue.get()
+
+
+                # Save player state
+                pygame.display.flip()
+
+                # Apply agent control
+                control, command = agent.run_step()
+                player.apply_control(control)
+
+                # Save data
+                measurements.append(measurement + (
+                    int(command),
+                    control.steer,
+                    control.throttle,
+                    control.brake,
+                    int(control.manual_gear_shift),
+                    control.gear,
+                ))
+
+                bird_view_images.append(np.concatenate([
+                    map_image, traffic_light_image, vehicle_image[...,None], walker_image[...,None], self_image[...,None]
+                ], axis=-1))
+
+                if args.render:
+                    # rgb_images.append(rgb_image)
+                    # depth_images.append(depth_image)
+                    # rgb_image.save_to_disk('tmp/%d.png'%t)
+                    # imsave(rgb_image, 'tmp/%d.png')
+                    # import pdb; pdb.set_trace()
+                    # print ("fuck")
+                    raise NotImplementedError
+                    # rgb_image = to_image(rgb_image, cc.Raw)
+                    # imsave('tmp/%d.png'%t, rgb_image)
+
+
+        # Destroy actors
+        print('destroying actors.')
+        for actor in actor_list:
+            actor.destroy()
+
+        # Save data
+        mkdir_p(os.path.dirname(dataset_name))
+        hdf5_file = h5py.File(dataset_name, mode='w')
+        hdf5_file.create_dataset(
+            "bird_view_images", data=bird_view_images, dtype=np.uint8
+        )
+
+        hdf5_file.create_dataset(
+            "measurements", data=measurements, dtype=np.float
+        )
+
+
+        # client.reload_world()
+        module_manager.clear_modules()
+
+
+if __name__ == '__main__':
+    parser = argparse.ArgumentParser()
+
+    parser.add_argument('-i', '--save-to-disk', action='store_true')
+    parser.add_argument('-C', '--town-name', choices=[
+        'Town01', 'Town02', 'Town03', 'Town04'
+    ], default='Town01')
+    parser.add_argument('-V', '--vehicle', default='vehicle.ford.mustang')
+    parser.add_argument('--render', action='store_true')
+    parser.add_argument('--num-vehicles', type=int, default=15)
+    parser.add_argument('--num-pedestrians', type=int, default=30)
+    parser.add_argument('--episode_length', type=int, default=2000)
+    parser.add_argument('--num_trajs', type=int, default=1)
+    parser.add_argument('--dataset_path', default='/raid0/dian/carla_0.9.4_data')
+
+    args = parser.parse_args()
+
+    main(args)