├── .gitignore ├── Airsim - Landscape Mountain ├── Behavioral_Cloning_Airsim_Autonomous_Car.ipynb ├── car.py └── test.py ├── Airsim ├── DAgger_record.py ├── README.md ├── notebooks │ ├── 0_Records.ipynb │ ├── 1_Landmarks.ipynb │ ├── 2_Generate_Trainset.ipynb │ ├── 3_Train_Model.ipynb │ └── 4_Path_Simulator.ipynb ├── requirements.txt ├── routes │ └── r1.csv ├── saved_models │ └── model-59.h5 ├── test.py ├── test_model.py └── utils │ ├── __index__.py │ ├── functions.py │ ├── normalizers.py │ └── route.py ├── Carla ├── examples │ ├── blueprint.py │ ├── camera.py │ ├── carla-data.py │ ├── generate_traffic.py │ ├── maps.py │ ├── retrive_data.py │ ├── route.py │ ├── spawnpoints.py │ ├── vehicle.py │ ├── waypoints-test.py │ └── waypoints.py ├── info │ ├── Frame Controls.txt │ └── blue details.txt ├── notebooks │ ├── Data_Analysis.ipynb │ ├── Generate_Trainset.ipynb │ └── Model.ipynb ├── retrive_data │ ├── agents │ │ ├── __init__.py │ │ ├── navigation │ │ │ ├── __init__.py │ │ │ ├── basic_agent.py │ │ │ ├── behavior_agent.py │ │ │ ├── behavior_types.py │ │ │ ├── controller.py │ │ │ ├── global_route_planner.py │ │ │ └── local_planner.py │ │ └── tools │ │ │ ├── __init__.py │ │ │ └── misc.py │ ├── generator.py │ ├── main.py │ └── misc.py └── waypoints │ ├── 1.npy │ ├── 2.npy │ ├── pos │ ├── pos.npy │ ├── w1 │ └── wayppp.npy ├── README.md └── assets ├── dagger.gif └── mountanous-auto-car.gif /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | pip-wheel-metadata/ 24 | share/python-wheels/ 25 | *.egg-info/ 26 | .installed.cfg 27 | *.egg 28 | MANIFEST 29 | 30 | # PyInstaller 31 | # Usually these files are written by a python script from a template 32 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 33 | *.manifest 34 | *.spec 35 | 36 | # Installer logs 37 | pip-log.txt 38 | pip-delete-this-directory.txt 39 | 40 | # Unit test / coverage reports 41 | htmlcov/ 42 | .tox/ 43 | .nox/ 44 | .coverage 45 | .coverage.* 46 | .cache 47 | nosetests.xml 48 | coverage.xml 49 | *.cover 50 | *.py,cover 51 | .hypothesis/ 52 | .pytest_cache/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | target/ 76 | 77 | # Jupyter Notebook 78 | .ipynb_checkpoints 79 | 80 | # IPython 81 | profile_default/ 82 | ipython_config.py 83 | 84 | # pyenv 85 | .python-version 86 | 87 | # pipenv 88 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 89 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 90 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 91 | # install all needed dependencies. 92 | #Pipfile.lock 93 | 94 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 95 | __pypackages__/ 96 | 97 | # Celery stuff 98 | celerybeat-schedule 99 | celerybeat.pid 100 | 101 | # SageMath parsed files 102 | *.sage.py 103 | 104 | # Environments 105 | .env 106 | .venv 107 | env/ 108 | venv/ 109 | ENV/ 110 | env.bak/ 111 | venv.bak/ 112 | 113 | # Spyder project settings 114 | .spyderproject 115 | .spyproject 116 | 117 | # Rope project settings 118 | .ropeproject 119 | 120 | # mkdocs documentation 121 | /site 122 | 123 | # mypy 124 | .mypy_cache/ 125 | .dmypy.json 126 | dmypy.json 127 | 128 | # Pyre type checker 129 | .pyre/ 130 | 131 | */model/* -------------------------------------------------------------------------------- /Airsim - Landscape Mountain/car.py: -------------------------------------------------------------------------------- 1 | import airsim 2 | import cv2 3 | import numpy as np 4 | import os 5 | import time 6 | import tempfile 7 | 8 | # connect to the AirSim simulator 9 | client = airsim.CarClient() 10 | client.confirmConnection() 11 | client.enableApiControl(True) 12 | print("API Control enabled: %s" % client.isApiControlEnabled()) 13 | car_controls = airsim.CarControls() 14 | 15 | tmp_dir = os.path.join(tempfile.gettempdir(), "airsim_car") 16 | print ("Saving images to %s" % tmp_dir) 17 | try: 18 | os.makedirs(tmp_dir) 19 | except OSError: 20 | if not os.path.isdir(tmp_dir): 21 | raise 22 | 23 | for idx in range(3): 24 | # get state of the car 25 | car_state = client.getCarState() 26 | print("Speed %d, Gear %d" % (car_state.speed, car_state.gear)) 27 | 28 | # go forward 29 | car_controls.throttle = 0.5 30 | car_controls.steering = 0 31 | client.setCarControls(car_controls) 32 | print("Go Forward") 33 | time.sleep(3) # let car drive a bit 34 | gps = client.getGpsData() 35 | print(gps) 36 | 37 | # Go forward + steer right 38 | car_controls.throttle = 0.5 39 | car_controls.steering = 1 40 | client.setCarControls(car_controls) 41 | print("Go Forward, steer right") 42 | time.sleep(3) # let car drive a bit 43 | 44 | # go reverse 45 | car_controls.throttle = -0.5 46 | car_controls.is_manual_gear = True 47 | car_controls.manual_gear = -1 48 | car_controls.steering = 0 49 | client.setCarControls(car_controls) 50 | print("Go reverse, steer right") 51 | time.sleep(3) # let car drive a bit 52 | car_controls.is_manual_gear = False # change back gear to auto 53 | car_controls.manual_gear = 0 54 | 55 | # apply brakes 56 | car_controls.brake = 1 57 | client.setCarControls(car_controls) 58 | print("Apply brakes") 59 | time.sleep(3) # let car drive a bit 60 | car_controls.brake = 0 #remove brake 61 | 62 | # # get camera images from the car 63 | # responses = client.simGetImages([ 64 | # airsim.ImageRequest("0", airsim.ImageType.DepthVis), #depth visualization image 65 | # airsim.ImageRequest("1", airsim.ImageType.DepthPerspective, True), #depth in perspective projection 66 | # airsim.ImageRequest("1", airsim.ImageType.Scene), #scene vision image in png format 67 | # airsim.ImageRequest("1", airsim.ImageType.Scene, False, False)]) #scene vision image in uncompressed RGB array 68 | # print('Retrieved images: %d' % len(responses)) 69 | 70 | # for response_idx, response in enumerate(responses): 71 | # filename = os.path.join(tmp_dir, f"{idx}_{response.image_type}_{response_idx}") 72 | 73 | # if response.pixels_as_float: 74 | # print("Type %d, size %d" % (response.image_type, len(response.image_data_float))) 75 | # airsim.write_pfm(os.path.normpath(filename + '.pfm'), airsim.get_pfm_array(response)) 76 | # elif response.compress: #png format 77 | # print("Type %d, size %d" % (response.image_type, len(response.image_data_uint8))) 78 | # airsim.write_file(os.path.normpath(filename + '.png'), response.image_data_uint8) 79 | # else: #uncompressed array 80 | # print("Type %d, size %d" % (response.image_type, len(response.image_data_uint8))) 81 | # img1d = np.fromstring(response.image_data_uint8, dtype=np.uint8) # get numpy array 82 | # img_rgb = img1d.reshape(response.height, response.width, 3) # reshape array to 3 channel image array H X W X 3 83 | # cv2.imwrite(os.path.normpath(filename + '.png'), img_rgb) # write to png 84 | 85 | #restore to original state 86 | client.reset() 87 | 88 | client.enableApiControl(False) 89 | -------------------------------------------------------------------------------- /Airsim - Landscape Mountain/test.py: -------------------------------------------------------------------------------- 1 | from tensorflow.keras.models import load_model 2 | import sys 3 | import numpy as np 4 | import glob 5 | import os 6 | import cv2 7 | import airsim 8 | import matplotlib.pyplot as plt 9 | 10 | if ('../../PythonClient/' not in sys.path): 11 | sys.path.insert(0, '../../PythonClient/') 12 | # from AirSimClient import * 13 | cv2.waitKey() 14 | # << Set this to the path of the model >> 15 | # If None, then the model with the lowest validation loss from training will be used 16 | MODEL_PATH = None 17 | 18 | if (MODEL_PATH == None): 19 | models = glob.glob('./model/models/*.h5') 20 | best_model = max(models, key=os.path.getctime) 21 | MODEL_PATH = r'D:\Users\Parsa Sam\Documents\GitHub\Autonomous-Car\Airsim\model\models\model_model.52-0.0002465.h5' 22 | 23 | print('Using model {0} for testing.'.format(MODEL_PATH)) 24 | print("press key") 25 | cv2.waitKey() 26 | 27 | 28 | 29 | model = load_model(MODEL_PATH) 30 | 31 | print("Model loaded") 32 | print("press key") 33 | cv2.waitKey() 34 | 35 | 36 | client = airsim.CarClient() 37 | client.confirmConnection() 38 | client.enableApiControl(True) 39 | car_controls = airsim.CarControls() 40 | print('Connection established!') 41 | 42 | print("press key") 43 | cv2.waitKey() 44 | 45 | 46 | car_controls.steering = 0 47 | car_controls.throttle = 0 48 | car_controls.brake = 0 49 | 50 | image_buf = np.zeros((1, 59, 255, 3)) 51 | state_buf = np.zeros((1,4)) 52 | 53 | print("press key") 54 | cv2.waitKey() 55 | 56 | 57 | 58 | def get_image(): 59 | image_response = client.simGetImages([airsim.ImageRequest(0, airsim.ImageType.Scene, False, False)])[0] 60 | image1d = np.fromstring(image_response.image_data_uint8, dtype=np.uint8) 61 | image_rgba = image1d.reshape(image_response.height, image_response.width, 3) 62 | return image_rgba[76:135,0:255,0:3].astype(float) / 255.0 63 | 64 | print("We got here") 65 | 66 | while (True): 67 | car_state = client.getCarState() 68 | 69 | if (car_state.speed < 5): 70 | car_controls.throttle = 0.5 71 | else: 72 | car_controls.throttle = 0.0 73 | 74 | image_buf[0] = get_image() 75 | state_buf[0] = np.array([car_controls.steering, car_controls.throttle, car_controls.brake, car_state.speed]) 76 | model_output = model.predict([image_buf, state_buf]) 77 | print(model_output) 78 | car_controls.steering = round(0.5 * float(model_output[0][0]), 2) 79 | 80 | print('Sending steering = {0}, throttle = {1}'.format(car_controls.steering, car_controls.throttle)) 81 | 82 | client.setCarControls(car_controls) -------------------------------------------------------------------------------- /Airsim/DAgger_record.py: -------------------------------------------------------------------------------- 1 | import airsim 2 | import numpy as np 3 | import os 4 | import time 5 | import tempfile 6 | import keyboard 7 | 8 | 9 | # connect to the AirSim simulator 10 | client = airsim.CarClient() 11 | client.confirmConnection() 12 | client.enableApiControl(True) 13 | print("API Control enabled: %s" % client.isApiControlEnabled()) 14 | car_controls = airsim.CarControls() 15 | 16 | tmp_dir = os.path.join(tempfile.gettempdir(), "airsim_car") 17 | print ("Saving images to %s" % tmp_dir) 18 | try: 19 | os.makedirs(tmp_dir) 20 | except OSError: 21 | if not os.path.isdir(tmp_dir): 22 | raise 23 | 24 | while True: 25 | # get state of the car 26 | car_state = client.getCarState() 27 | print("Speed %d, Gear %d" % (car_state.speed, car_state.gear)) 28 | 29 | car_controls.throttle = 0.5 30 | car_controls.steering = 0 31 | client.setCarControls(car_controls) 32 | print("Go Forward") 33 | 34 | if keyboard.is_pressed(" "): 35 | # if client.isRecording(): 36 | client.enableApiControl(False) 37 | client.startRecording() 38 | time.sleep(5) 39 | client.stopRecording() 40 | client.enableApiControl(True) 41 | 42 | client.reset() 43 | client.enableApiControl(False) 44 | -------------------------------------------------------------------------------- /Airsim/README.md: -------------------------------------------------------------------------------- 1 | # Autonomous-Car -------------------------------------------------------------------------------- /Airsim/requirements.txt: -------------------------------------------------------------------------------- 1 | keyboard=0.13.5 -------------------------------------------------------------------------------- /Airsim/routes/r1.csv: -------------------------------------------------------------------------------- 1 | POS_X,POS_Y,Speed 2 | -126.317,-111.717,1.91488e-07 3 | -126.317,-111.717,-5.88574e-09 4 | -126.317,-111.717,-2.28387e-07 5 | -126.317,-111.717,-2.59475e-07 6 | -126.317,-111.717,5.03762e-08 7 | -126.317,-111.717,-2.3677e-07 8 | -126.317,-111.717,-6.32112e-08 9 | -126.317,-111.717,1.83865e-07 10 | -126.317,-111.717,-4.76475e-08 11 | -126.317,-111.717,-8.12161e-08 12 | -126.317,-111.717,-2.8844e-07 13 | -126.317,-111.717,1.06945e-07 14 | -126.317,-111.717,-1.997e-07 15 | -126.317,-111.717,2.03204e-07 16 | -126.317,-111.717,-3.80813e-07 17 | -126.317,-111.717,-1.95236e-07 18 | -126.317,-111.717,2.06196e-07 19 | -126.317,-111.717,0.11901 20 | -126.316,-111.7,0.384502 21 | -126.313,-111.658,0.672638 22 | -126.309,-111.594,0.889475 23 | -126.304,-111.515,1.11262 24 | -126.298,-111.419,1.27783 25 | -126.291,-111.307,1.45146 26 | -126.292,-111.182,1.57684 27 | -126.287,-111.048,1.72642 28 | -126.279,-110.905,1.83346 29 | -126.271,-110.748,1.96613 30 | -126.261,-110.575,2.06203 31 | -126.251,-110.404,2.18195 32 | -126.241,-110.221,2.29439 33 | -126.231,-110.028,2.46896 34 | -126.219,-109.819,2.63029 35 | -126.206,-109.595,2.8351 36 | -126.193,-109.352,3.01645 37 | -126.179,-109.097,3.23145 38 | -126.164,-108.824,3.41881 39 | -126.147,-108.529,3.64356 40 | -126.13,-108.214,3.84055 41 | -126.112,-107.89,4.06269 42 | -126.093,-107.554,4.25988 43 | -126.073,-107.188,4.48046 44 | -126.052,-106.806,4.69008 45 | -126.03,-106.408,4.90498 46 | -126.007,-105.994,5.15793 47 | -125.982,-105.551,5.40784 48 | -125.957,-105.103,5.59205 49 | -125.931,-104.637,5.40171 50 | -125.906,-104.186,5.73717 51 | -125.88,-103.704,6.04454 52 | -125.851,-103.196,6.46229 53 | -125.822,-102.673,6.76185 54 | -125.791,-102.102,6.9498 55 | -125.759,-101.514,7.31982 56 | -125.725,-100.904,7.50969 57 | -125.69,-100.268,7.57814 58 | -125.655,-99.6342,7.84926 59 | -125.62,-98.9922,7.90721 60 | -125.588,-98.3166,7.79473 61 | -125.582,-97.6931,7.98636 62 | -125.58,-97.0678,7.76512 63 | -125.572,-96.4529,7.68923 64 | -125.564,-95.845,7.44094 65 | -125.57,-95.2323,7.38038 66 | -125.604,-94.6166,7.26433 67 | -125.631,-94.0383,7.13606 68 | -125.653,-93.4666,6.96642 69 | -125.675,-92.9365,6.99871 70 | -125.695,-92.4107,6.87192 71 | -125.718,-91.8652,6.83955 72 | -125.738,-91.3303,6.81997 73 | -125.759,-90.8175,6.85786 74 | -125.78,-90.2709,6.91911 75 | -125.801,-89.7441,6.94379 76 | -125.822,-89.2177,7.12032 77 | -125.844,-88.6473,7.31479 78 | -125.866,-88.0995,7.50427 79 | -125.9,-87.5251,7.3723 80 | -125.955,-86.9734,7.43892 81 | -126.004,-86.4113,7.78244 82 | -126.053,-85.8216,7.96731 83 | -126.101,-85.2378,8.31898 84 | -126.142,-84.6369,8.48613 85 | -126.162,-83.9986,8.80498 86 | -126.173,-83.3317,8.97728 87 | -126.192,-82.6488,9.30912 88 | -126.211,-81.9224,9.50627 89 | -126.231,-81.2102,9.79867 90 | -126.253,-80.4747,9.98611 91 | -126.275,-79.7199,10.2686 92 | -126.298,-78.9323,10.4571 93 | -126.321,-78.1489,10.7255 94 | -126.344,-77.3587,10.9129 95 | -126.367,-76.5453,11.1595 96 | -126.391,-75.7258,11.3527 97 | -126.417,-74.8668,11.5873 98 | -126.444,-73.9341,11.7972 99 | -126.47,-73.0514,12.0256 100 | -126.495,-72.1808,12.2175 101 | -126.522,-71.2705,12.4294 102 | -126.549,-70.3454,12.63 103 | -126.577,-69.3972,12.8366 104 | -126.606,-68.4382,13.0373 105 | -126.633,-67.5017,13.2311 106 | -126.662,-66.5248,13.1903 107 | -126.69,-65.5599,13.184 108 | -126.719,-64.5936,13.3386 109 | -126.747,-63.6388,13.5545 110 | -126.776,-62.6585,13.7661 111 | -126.807,-61.619,13.9811 112 | -126.837,-60.6058,14.1819 113 | -126.867,-59.5899,14.3756 114 | -126.898,-58.5284,14.5706 115 | -126.928,-57.5073,14.7517 116 | -126.959,-56.4715,14.9296 117 | -126.99,-55.417,15.1053 118 | -127.023,-54.3088,15.2845 119 | -127.056,-53.2052,15.458 120 | -127.089,-52.0842,15.6297 121 | -127.125,-50.8966,15.8071 122 | -127.158,-49.772,15.9711 123 | -127.191,-48.6767,16.1276 124 | -127.223,-47.5768,16.2816 125 | -127.256,-46.4684,16.434 126 | -127.289,-45.3645,16.5832 127 | -127.323,-44.2491,16.7283 128 | -127.355,-43.1503,16.7601 129 | -127.388,-42.0632,16.4048 130 | -127.419,-41.0055,15.9691 131 | -127.451,-39.9541,15.6801 132 | -127.483,-38.8845,15.4065 133 | -127.513,-37.8596,15.117 134 | -127.542,-36.8808,14.8185 135 | -127.571,-35.9164,14.5054 136 | -127.598,-34.9942,14.1896 137 | -127.625,-34.0857,13.8639 138 | -127.651,-33.2085,13.5361 139 | -127.677,-32.3456,13.2014 140 | -127.702,-31.5075,12.8648 141 | -127.725,-30.7056,12.5321 142 | -127.749,-29.8987,12.187 143 | -127.772,-29.1156,11.8417 144 | -127.795,-28.3528,11.4955 145 | -127.816,-27.6265,11.0677 146 | -127.837,-26.926,10.7438 147 | -127.857,-26.2491,10.5667 148 | -127.876,-25.5946,10.3468 149 | -127.895,-24.9472,10.14 150 | -127.913,-24.3179,9.94846 151 | -127.931,-23.7027,9.77057 152 | -127.949,-23.0992,9.60478 153 | -127.971,-22.3581,9.41259 154 | -127.989,-21.7321,9.25951 155 | -128.005,-21.1633,9.12854 156 | -128.022,-20.607,9.00614 157 | -128.038,-20.0577,8.89159 158 | -128.054,-19.5096,8.7825 159 | -128.063,-18.9432,8.66962 160 | -128.056,-18.3773,8.56408 161 | -128.045,-17.8255,8.47314 162 | -128.039,-17.297,8.38521 163 | -128.034,-16.7731,8.30252 164 | -128.028,-16.2575,8.22297 165 | -128.023,-15.7497,8.14943 166 | -128.017,-15.2361,8.07474 167 | -128.011,-14.7472,8.01009 168 | -128.006,-14.2561,7.94198 169 | -128.0,-13.7685,7.88257 170 | -127.995,-13.2877,7.81832 171 | -127.99,-12.8149,7.76431 172 | -127.984,-12.3427,7.70505 173 | -127.979,-11.8729,7.65309 174 | -127.974,-11.4065,7.59828 175 | -127.968,-10.8936,7.54571 176 | -127.962,-10.4398,7.48806 177 | -127.939,-9.90051,7.41927 178 | -127.893,-9.38208,7.36018 179 | -127.825,-8.91276,7.21782 180 | -127.732,-8.47011,7.14696 181 | -127.603,-8.02404,6.98651 182 | -127.437,-7.59269,6.92616 183 | -127.228,-7.16152,6.77728 184 | -126.984,-6.75048,6.76473 185 | -126.701,-6.35453,6.6072 186 | -126.383,-5.98225,6.61626 187 | -126.02,-5.62614,6.42925 188 | -125.628,-5.30681,6.45541 189 | -125.219,-5.02945,6.30435 190 | -124.775,-4.78473,6.18459 191 | -124.309,-4.58201,6.03734 192 | -123.818,-4.42187,6.05064 193 | -123.322,-4.30897,5.94556 194 | -122.807,-4.21956,5.83494 195 | -122.296,-4.12223,5.7845 196 | -121.779,-4.00232,5.65285 197 | -121.272,-3.8838,5.69689 198 | -120.77,-3.76593,5.71683 199 | -120.273,-3.64834,5.85954 200 | -119.765,-3.52797,5.77161 201 | -119.263,-3.4083,5.857 202 | -118.754,-3.28769,6.07794 203 | -118.218,-3.15957,6.35904 204 | -117.66,-3.0261,6.7264 205 | -117.047,-2.87901,7.07561 206 | -116.411,-2.72657,7.41549 207 | -115.739,-2.58287,7.82783 208 | -115.004,-2.47006,8.04881 209 | -114.268,-2.40975,8.30604 210 | -113.503,-2.39541,8.6358 211 | -112.726,-2.40075,8.93609 212 | -111.917,-2.39745,9.23123 213 | -111.095,-2.39529,9.51955 214 | -110.243,-2.37961,9.79382 215 | -109.36,-2.35973,10.0698 216 | -108.453,-2.34062,10.3441 217 | -107.501,-2.31958,10.6194 218 | -106.514,-2.29751,10.8984 219 | -105.52,-2.27493,11.1604 220 | -104.504,-2.25179,11.423 221 | -103.488,-2.22852,11.6764 222 | -102.449,-2.20477,11.9244 223 | -101.39,-2.18049,12.1724 224 | -100.318,-2.15596,12.4156 225 | -99.2095,-2.1305,12.6585 226 | -98.1169,-2.10546,12.8915 227 | -96.9713,-2.07916,13.1292 228 | -95.8139,-2.05256,13.2655 229 | -94.6674,-2.02613,13.2553 230 | -93.5399,-2.00017,13.3459 231 | -92.3973,-1.97384,13.5987 232 | -91.2402,-1.94718,13.843 233 | -90.0667,-1.9201,14.0783 234 | -88.8882,-1.89288,14.3046 235 | -87.666,-1.86462,14.5289 236 | -86.45,-1.83648,14.7433 237 | -85.2233,-1.80807,14.9516 238 | -83.927,-1.77802,15.1641 239 | -82.6452,-1.74828,15.3672 240 | -81.2591,-1.71609,15.5801 241 | -79.9107,-1.68474,15.7812 242 | -78.5766,-1.6537,15.9748 243 | -77.2438,-1.62267,16.1636 244 | -75.8953,-1.59124,16.3503 245 | -74.5522,-1.55991,16.5323 246 | -73.1831,-1.52795,16.7142 247 | -71.7731,-1.49501,16.8979 248 | -70.4277,-1.46353,17.0701 249 | -69.0185,-1.43056,17.2475 250 | -67.6307,-1.39803,17.4193 251 | -66.175,-1.3639,17.5966 252 | -64.7648,-1.33081,17.7659 253 | -63.3279,-1.29706,17.9359 254 | -61.9111,-1.26376,18.1011 255 | -60.4836,-1.23018,18.2654 256 | -59.054,-1.19652,18.4279 257 | -57.609,-1.16247,18.59 258 | -56.1582,-1.12826,18.7508 259 | -54.6986,-1.09381,18.9106 260 | -53.2533,-1.05967,19.0671 261 | -51.7185,-1.02339,19.2312 262 | -50.2152,-0.987818,19.3902 263 | -48.7107,-0.95219,19.5475 264 | -47.2519,-0.917618,19.6983 265 | -45.7672,-0.882404,19.8503 266 | -44.2848,-0.847216,20.0004 267 | -42.8042,-0.812041,20.1489 268 | -41.2971,-0.776206,20.2984 269 | -39.8167,-0.740981,20.3566 270 | -38.3644,-0.706398,20.3309 271 | -36.8845,-0.671137,20.3915 272 | -35.3501,-0.63456,20.5699 273 | -33.8472,-0.598716,20.7363 274 | -32.3112,-0.562062,20.8973 275 | -30.8103,-0.526219,21.0469 276 | -29.3074,-0.490301,21.1899 277 | -27.7883,-0.453966,21.3283 278 | -26.3066,-0.418499,21.4583 279 | -24.7763,-0.381838,21.5879 280 | -23.2854,-0.346094,21.7102 281 | -21.7262,-0.308682,21.8346 282 | -20.1785,-0.271518,21.9549 283 | -18.6532,-0.234865,22.0708 284 | -17.0935,-0.197359,22.1869 285 | -15.5433,-0.160053,22.3002 286 | -14.0109,-0.123149,22.4104 287 | -12.4682,-0.0859705,22.5196 288 | -10.9133,-0.0484713,22.6283 289 | -9.36891,-0.0112012,22.7349 290 | -7.83392,0.0258671,22.8396 291 | -6.29567,0.0630389,22.942 292 | -4.76155,0.094771,23.0418 293 | -3.18814,0.124323,23.1461 294 | -1.63506,0.151267,23.2481 295 | -0.107876,0.174948,23.3477 296 | 1.4199,0.196277,23.4465 297 | 2.9902,0.216381,23.5473 298 | 4.53914,0.23497,23.6461 299 | 6.08601,0.252722,23.7424 300 | 7.61417,0.264899,23.8288 301 | 9.14711,0.26424,23.9232 302 | 10.6661,0.259327,24.0167 303 | 12.2071,0.248026,24.1112 304 | 13.7829,0.229505,24.2077 305 | 15.3588,0.205151,24.3039 306 | 16.9142,0.176821,24.3984 307 | 18.4449,0.145982,24.4909 308 | 19.9683,0.113287,24.5824 309 | 21.5094,0.078858,24.6744 310 | 23.0232,0.0441808,24.7643 311 | 24.5282,0.00917988,24.8531 312 | 26.0596,-0.0267567,24.9429 313 | 27.6295,-0.0637855,25.0345 314 | 29.1778,-0.100396,25.1244 315 | 30.6983,-0.136381,25.2121 316 | 32.2644,-0.173442,25.302 317 | 33.7921,-0.209575,25.3074 318 | 35.3079,-0.245399,25.119 319 | 36.7982,-0.280596,24.7331 320 | 38.2617,-0.315147,24.3398 321 | 39.6935,-0.348948,24.0214 322 | 41.109,-0.38238,23.731 323 | 42.4858,-0.410976,23.4526 324 | 43.8599,-0.429068,23.2056 325 | 45.2148,-0.444365,23.1017 326 | 46.559,-0.454971,23.062 327 | 47.8868,-0.460115,23.0142 328 | 49.2122,-0.460538,22.9661 329 | 50.5639,-0.45705,22.9161 330 | 51.8959,-0.450634,22.8661 331 | 53.2517,-0.441843,22.8253 332 | 54.5538,-0.431827,22.8086 333 | 55.8519,-0.420735,22.8111 334 | 57.1639,-0.408731,22.8289 335 | 58.473,-0.396198,22.859 336 | 59.7737,-0.38337,22.8988 337 | 61.0876,-0.370155,22.9473 338 | 62.382,-0.35697,23.0019 339 | 63.6859,-0.343579,23.0625 340 | 65.0263,-0.329741,23.1298 341 | 66.3536,-0.315992,23.2007 342 | 67.7016,-0.301999,23.2586 343 | 69.0462,-0.28802,23.2217 344 | 70.3606,-0.274343,23.088 345 | 71.6821,-0.260587,22.7855 346 | 72.9696,-0.247187,22.4088 347 | 74.2298,-0.234082,22.0379 348 | 75.4853,-0.221051,21.7145 349 | 76.7225,-0.208248,21.403 350 | 77.9242,-0.195855,21.0901 351 | 79.1018,-0.183756,20.7743 352 | 80.3027,-0.171565,20.4432 353 | 81.4481,-0.165127,20.1175 354 | 82.7043,-0.17033,19.7514 355 | 83.8485,-0.177353,19.4094 356 | 84.9397,-0.187876,19.0762 357 | 86.029,-0.203021,18.737 358 | 87.0723,-0.221517,18.4058 359 | 88.1143,-0.243216,18.0686 360 | 89.1284,-0.266782,17.7343 361 | 90.1298,-0.291886,17.3979 362 | 91.0995,-0.317512,17.0663 363 | 92.0639,-0.343969,16.712 364 | 93.0164,-0.370806,16.3397 365 | 93.9348,-0.397172,15.9746 366 | 94.832,-0.423277,15.5564 367 | 95.7108,-0.44909,15.1254 368 | 96.5655,-0.474369,14.6998 369 | 97.4053,-0.499336,14.2747 370 | 98.2138,-0.523465,13.8588 371 | 99.0048,-0.547137,13.445 372 | 99.7717,-0.570144,13.037 373 | 100.512,-0.592402,12.6362 374 | 101.236,-0.614189,12.2377 375 | 101.937,-0.63533,11.8446 376 | 102.614,-0.655747,11.4586 377 | 103.259,-0.67526,11.0835 378 | 103.89,-0.694339,10.7105 379 | 104.501,-0.712863,10.342 380 | 105.088,-0.730657,10.103 381 | 105.657,-0.747916,9.96825 382 | 106.222,-0.765071,9.82455 383 | 106.78,-0.782029,9.68753 384 | 107.332,-0.798798,9.55774 385 | 107.877,-0.815383,9.43477 386 | 108.417,-0.831792,9.31817 387 | 108.941,-0.847751,9.2094 388 | 109.469,-0.863805,9.10433 389 | 109.986,-0.879549,9.00526 390 | 110.496,-0.895079,8.91125 391 | 111.009,-0.910688,8.82023 392 | 111.512,-0.926009,8.73402 393 | 112.009,-0.941131,8.65183 394 | 112.497,-0.95601,8.57361 395 | 112.983,-0.970804,8.49822 396 | 113.459,-0.985324,8.42642 397 | 113.932,-0.999742,8.35714 398 | 114.402,-1.01405,8.29022 399 | 114.874,-1.02844,8.2247 400 | 115.34,-1.04265,8.16162 401 | 115.806,-1.05686,8.10003 402 | 116.265,-1.07087,8.04072 403 | 116.715,-1.08459,7.98385 404 | 117.168,-1.09841,7.92781 405 | 117.614,-1.112,7.87383 406 | 118.058,-1.12555,7.82105 407 | 118.501,-1.13908,7.76933 408 | 118.948,-1.1527,7.71757 409 | 119.38,-1.17191,7.66146 410 | 119.807,-1.20299,7.59748 411 | 120.233,-1.25002,7.52413 412 | 120.652,-1.31612,7.44119 413 | 121.061,-1.40336,7.3487 414 | 121.458,-1.51452,7.24566 415 | 121.843,-1.65198,7.14258 416 | 122.21,-1.81378,7.0582 417 | 122.567,-2.00117,6.98313 418 | 122.905,-2.20968,6.9135 419 | 123.231,-2.44292,6.84641 420 | 123.531,-2.69119,6.7836 421 | 123.812,-2.95746,6.72337 422 | 124.068,-3.23506,6.6665 423 | 124.304,-3.52895,6.61146 424 | 124.522,-3.84088,6.55755 425 | 124.716,-4.16125,6.50611 426 | 124.884,-4.48803,6.45683 427 | 125.032,-4.82801,6.44764 428 | 125.164,-5.16859,6.475 429 | 125.296,-5.51597,6.48595 430 | 125.428,-5.86748,6.49077 431 | 125.568,-6.21321,6.52937 432 | 125.72,-6.57315,6.56073 433 | 125.87,-6.92559,6.65504 434 | 126.019,-7.27751,6.70451 435 | 126.162,-7.63331,6.80693 436 | 126.297,-8.00197,6.85928 437 | 126.418,-8.38064,6.97753 438 | 126.528,-8.76697,7.07052 439 | 126.63,-9.16467,7.19078 440 | 126.717,-9.57135,7.29428 441 | 126.791,-9.98533,7.44539 442 | 126.871,-10.4214,7.572 443 | 126.958,-10.8494,7.60232 444 | 127.047,-11.2786,7.60138 445 | 127.137,-11.7121,7.59212 446 | 127.226,-12.1412,7.76638 447 | 127.316,-12.5824,7.97778 448 | 127.399,-13.0339,8.16532 449 | 127.47,-13.4993,8.34288 450 | 127.524,-13.9803,8.53451 451 | 127.565,-14.474,8.73995 452 | 127.612,-14.9767,8.93862 453 | 127.664,-15.4919,9.13878 454 | 127.716,-16.0173,9.33544 455 | 127.77,-16.5609,9.53159 456 | 127.823,-17.1054,9.72127 457 | 127.879,-17.6759,9.91032 458 | 127.931,-18.2519,10.0841 459 | 127.97,-18.8328,10.2425 460 | 127.994,-19.4198,10.419 461 | 128.015,-20.0224,10.5964 462 | 128.038,-20.6396,10.7758 463 | 128.061,-21.2597,10.9512 464 | 128.083,-21.8879,11.1241 465 | 128.105,-22.5286,11.2959 466 | 128.126,-23.1794,11.4661 467 | 128.149,-23.8498,11.637 468 | 128.171,-24.5271,11.8057 469 | 128.193,-25.1965,11.9686 470 | 128.217,-25.8945,12.1347 471 | 128.239,-26.5839,12.2953 472 | 128.263,-27.2872,12.4558 473 | 128.287,-28.0027,12.6158 474 | 128.31,-28.7223,12.7736 475 | 128.336,-29.4804,12.9367 476 | 128.36,-30.2247,13.0195 477 | 128.385,-30.9705,12.9913 478 | 128.414,-31.8336,12.9791 479 | 128.439,-32.5758,13.1123 480 | 128.464,-33.3309,13.2878 481 | 128.494,-34.2296,13.4891 482 | 128.52,-35.0177,13.6589 483 | 128.546,-35.8082,13.8235 484 | 128.572,-36.5938,13.9819 485 | 128.599,-37.3834,14.1361 486 | 128.626,-38.1857,14.2882 487 | 128.653,-39.0002,14.4382 488 | 128.68,-39.8138,14.584 489 | 128.708,-40.6502,14.7299 490 | 128.736,-41.4827,14.8716 491 | 128.764,-42.322,15.011 492 | 128.793,-43.1841,15.151 493 | 128.821,-44.0291,15.2852 494 | 128.85,-44.8841,15.4184 495 | 128.878,-45.7451,15.5498 496 | 128.908,-46.6175,15.6806 497 | 128.937,-47.4894,15.8089 498 | 128.966,-48.3587,15.9348 499 | 128.996,-49.2533,16.0622 500 | 129.03,-50.291,16.2021 501 | 129.052,-51.2087,16.3258 502 | 129.072,-52.1492,16.4536 503 | 129.091,-53.0822,16.5788 504 | 129.108,-54.0153,16.7025 505 | 129.124,-54.9494,16.8247 506 | 129.138,-55.886,16.9456 507 | 129.152,-56.8259,17.0657 508 | 129.166,-57.7694,17.1848 509 | 129.179,-58.7321,17.305 510 | 129.192,-59.7068,17.4253 511 | 129.206,-60.6848,17.5447 512 | 129.219,-61.6704,17.6638 513 | 129.233,-62.6429,17.7801 514 | 129.248,-63.7519,17.9114 515 | 129.261,-64.752,18.0285 516 | 129.275,-65.7622,18.1457 517 | 129.289,-66.7752,18.2621 518 | 129.303,-67.7821,18.3768 519 | 129.317,-68.8172,18.4936 520 | 129.331,-69.8389,18.6078 521 | 129.345,-70.8754,18.7228 522 | 129.359,-71.9156,18.8371 523 | 129.374,-72.9617,18.9471 524 | 129.382,-74.0335,19.0614 525 | 129.389,-75.0849,19.1742 526 | 129.395,-76.143,19.2868 527 | 129.4,-77.1975,19.3982 528 | 129.403,-78.273,19.5108 529 | 129.405,-79.3436,19.6221 530 | 129.406,-80.424,19.7336 531 | 129.407,-81.5178,19.8455 532 | 129.407,-82.6124,19.9567 533 | 129.408,-83.7022,20.0665 534 | 129.408,-84.8298,20.1158 535 | 129.408,-85.9395,20.0773 536 | 129.408,-87.0681,19.9384 537 | 129.408,-88.1734,19.66 538 | 129.408,-89.2767,19.2845 539 | 129.408,-90.3683,18.9145 540 | 129.408,-91.4162,18.6307 541 | 129.408,-92.4459,18.3631 542 | 129.408,-93.4565,18.0873 543 | 129.408,-94.4615,17.8012 544 | 129.408,-95.437,17.5128 545 | 129.408,-96.4033,17.2173 546 | 129.408,-97.3295,16.925 547 | 129.408,-98.2824,16.6156 548 | 129.407,-99.1958,16.3107 549 | 129.407,-100.101,16.0008 550 | 129.407,-100.997,15.6861 551 | 129.407,-101.861,15.3761 552 | 129.406,-102.712,15.0635 553 | 129.402,-103.545,14.7504 554 | 129.398,-104.361,14.4491 555 | 129.393,-105.161,14.1314 556 | 129.388,-105.971,13.7476 557 | 129.383,-106.737,13.389 558 | 129.377,-107.478,13.0104 559 | 129.371,-108.198,12.7382 560 | 129.365,-108.9,12.5519 561 | 129.36,-109.588,12.3539 562 | 129.354,-110.267,12.1646 563 | 129.348,-110.935,11.9837 564 | 129.342,-111.598,11.809 565 | 129.337,-112.259,11.6399 566 | 129.331,-112.901,11.4805 567 | 129.325,-113.537,11.3267 568 | 129.32,-114.155,11.1816 569 | 129.315,-114.784,11.0379 570 | 129.309,-115.386,10.9044 571 | 129.304,-115.992,10.7732 572 | 129.299,-116.588,10.6478 573 | 129.293,-117.182,10.526 574 | 129.281,-117.783,10.4024 575 | 129.258,-118.357,10.2808 576 | 129.218,-118.928,10.1546 577 | 129.158,-119.494,10.0211 578 | 129.074,-120.051,9.91717 579 | 128.97,-120.603,9.75405 580 | 128.835,-121.129,9.59522 581 | 128.667,-121.646,9.45515 582 | 128.474,-122.138,9.32439 583 | 128.251,-122.615,9.2094 584 | 127.993,-123.077,9.10715 585 | 127.706,-123.508,9.02111 586 | 127.383,-123.921,8.95083 587 | 127.033,-124.302,8.8967 588 | 126.656,-124.653,8.86325 589 | 126.247,-124.978,8.8595 590 | 125.818,-125.271,8.92116 591 | 125.346,-125.559,9.02595 592 | 124.86,-125.839,9.07228 593 | 124.379,-126.104,9.07005 594 | 123.883,-126.361,9.05154 595 | 123.379,-126.601,9.20536 596 | 122.845,-126.838,9.38995 597 | 122.289,-127.056,9.55285 598 | 121.699,-127.252,9.69221 599 | 121.079,-127.422,9.82483 600 | 120.409,-127.55,9.97894 601 | 119.712,-127.638,10.1594 602 | 118.93,-127.686,10.3557 603 | 118.154,-127.692,10.614 604 | 117.325,-127.681,10.8446 605 | 116.461,-127.664,11.1082 606 | 115.575,-127.64,11.3293 607 | 114.686,-127.613,11.5936 608 | 113.771,-127.584,11.8015 609 | 112.833,-127.554,12.0605 610 | 111.879,-127.524,12.265 611 | 110.915,-127.502,12.4935 612 | 109.95,-127.496,12.5379 613 | 109.002,-127.489,12.5468 614 | 108.068,-127.485,12.7021 615 | 107.111,-127.482,12.9406 616 | 106.134,-127.48,13.1675 617 | 105.137,-127.479,13.3909 618 | 104.125,-127.477,13.6063 619 | 103.09,-127.476,13.8178 620 | 102.064,-127.475,14.0187 621 | 100.963,-127.473,14.2258 622 | 99.8945,-127.472,14.4195 623 | 98.7971,-127.471,14.6067 624 | 97.7316,-127.469,14.7572 625 | 96.6428,-127.468,14.7624 626 | 95.5746,-127.467,14.4274 627 | 94.5471,-127.465,13.9532 628 | 93.5433,-127.464,13.5654 629 | 92.4016,-127.463,13.1874 630 | 91.4265,-127.462,12.8387 631 | 90.4766,-127.46,12.4765 632 | 89.5851,-127.459,12.1177 633 | 88.6855,-127.446,11.7323 634 | 87.8474,-127.413,11.3459 635 | 87.0286,-127.347,10.9343 636 | 86.2228,-127.237,10.485 637 | 85.4263,-127.066,9.98755 638 | 84.6571,-126.825,9.46825 639 | 83.9279,-126.513,8.92545 640 | 83.27,-126.141,8.16005 641 | 82.6841,-125.712,7.38156 642 | 82.208,-125.26,6.41719 643 | 81.8631,-124.838,5.32471 644 | 81.6193,-124.447,4.32591 645 | 81.452,-124.116,3.51278 646 | 81.3436,-123.841,2.73013 647 | 81.2803,-123.617,1.96045 648 | 81.2504,-123.454,1.23111 649 | 81.2405,-123.354,0.751164 650 | 81.2406,-123.27,1.03176 651 | 81.2453,-123.156,1.30073 652 | 81.2543,-123.019,1.60606 653 | 81.2695,-122.858,1.89012 654 | 81.278,-122.658,2.26449 655 | 81.2615,-122.448,2.50821 656 | 81.2227,-122.215,2.79201 657 | 81.1777,-121.961,3.03117 658 | 81.1296,-121.693,3.30059 659 | 81.0778,-121.405,3.53067 660 | 81.0217,-121.094,3.7938 661 | 80.9622,-120.764,4.02308 662 | 80.8976,-120.407,4.16761 663 | 80.833,-120.049,4.16094 664 | 80.7695,-119.697,4.3032 665 | 80.6998,-119.311,4.56957 666 | 80.6268,-118.907,4.84369 667 | 80.5497,-118.48,5.17278 668 | 80.4689,-118.033,5.56937 669 | 80.3892,-117.561,5.9182 670 | 80.3307,-117.054,6.22696 671 | 80.3072,-116.515,6.48454 672 | 80.3302,-115.957,6.7187 673 | 80.3735,-115.383,7.06059 674 | 80.4002,-114.802,7.36797 675 | 80.4218,-114.193,7.75704 676 | 80.4438,-113.567,7.95944 677 | 80.4665,-112.915,8.33295 678 | 80.4905,-112.224,8.53217 679 | 80.515,-111.513,8.89276 680 | 80.5395,-110.802,9.07028 681 | 80.5655,-110.048,9.41725 682 | 80.5928,-109.253,9.60389 683 | 80.6197,-108.47,9.92794 684 | 80.6469,-107.674,10.095 685 | 80.6747,-106.862,10.406 686 | 80.7031,-106.033,10.5825 687 | 80.7321,-105.187,10.8697 688 | 80.7622,-104.306,11.0504 689 | 80.7926,-103.417,11.3296 690 | 80.8233,-102.519,11.5099 691 | 80.856,-101.559,11.7855 692 | 80.8874,-100.64,11.9653 693 | 80.9203,-99.6752,12.2144 694 | 80.9527,-98.7255,12.404 695 | 80.985,-97.7604,12.6258 696 | 81.0114,-96.7611,12.827 697 | 81.0387,-95.7639,13.0429 698 | 81.066,-94.7366,13.2446 699 | 81.0893,-93.7001,13.3861 700 | 81.1138,-92.6259,13.3472 701 | 81.1365,-91.5789,13.4429 702 | -------------------------------------------------------------------------------- /Airsim/saved_models/model-59.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TroddenSpade/Flexible-Conditional-Imitation-Learning/7ce40ffeac69f3ed09afa3523ef32adb91ad5355/Airsim/saved_models/model-59.h5 -------------------------------------------------------------------------------- /Airsim/test.py: -------------------------------------------------------------------------------- 1 | import airsim 2 | import cv2 3 | import numpy as np 4 | import os 5 | import time 6 | import tempfile 7 | 8 | # connect to the AirSim simulator 9 | client = airsim.CarClient() 10 | client.confirmConnection() 11 | client.enableApiControl(True) 12 | print("API Control enabled: %s" % client.isApiControlEnabled()) 13 | car_controls = airsim.CarControls() 14 | 15 | tmp_dir = os.path.join(tempfile.gettempdir(), "airsim_car") 16 | print ("Saving images to %s" % tmp_dir) 17 | try: 18 | os.makedirs(tmp_dir) 19 | except OSError: 20 | if not os.path.isdir(tmp_dir): 21 | raise 22 | 23 | for idx in range(3): 24 | # get state of the car 25 | car_state = client.getCarState() 26 | print(car_state) 27 | gps = client.getGpsData() 28 | print(gps) 29 | 30 | # go forward 31 | car_controls.throttle = 0.5 32 | car_controls.steering = 0 33 | client.setCarControls(car_controls) 34 | print("Go Forward") 35 | time.sleep(10) # let car drive a bit 36 | 37 | # # Go forward + steer right 38 | # car_controls.throttle = 0.5 39 | # car_controls.steering = 1 40 | # client.setCarControls(car_controls) 41 | # print("Go Forward, steer right") 42 | # time.sleep(3) # let car drive a bit 43 | 44 | # # go reverse 45 | # car_controls.throttle = -0.5 46 | # car_controls.is_manual_gear = True 47 | # car_controls.manual_gear = -1 48 | # car_controls.steering = 0 49 | # client.setCarControls(car_controls) 50 | # print("Go reverse, steer right") 51 | # time.sleep(3) # let car drive a bit 52 | # car_controls.is_manual_gear = False # change back gear to auto 53 | # car_controls.manual_gear = 0 54 | 55 | # apply brakes 56 | car_controls.brake = 1 57 | client.setCarControls(car_controls) 58 | print("Apply brakes") 59 | time.sleep(10) # let car drive a bit 60 | car_controls.brake = 0 #remove brake 61 | 62 | # # get camera images from the car 63 | # responses = client.simGetImages([ 64 | # airsim.ImageRequest("0", airsim.ImageType.DepthVis), #depth visualization image 65 | # airsim.ImageRequest("1", airsim.ImageType.DepthPerspective, True), #depth in perspective projection 66 | # airsim.ImageRequest("1", airsim.ImageType.Scene), #scene vision image in png format 67 | # airsim.ImageRequest("1", airsim.ImageType.Scene, False, False)]) #scene vision image in uncompressed RGB array 68 | # print('Retrieved images: %d' % len(responses)) 69 | 70 | # for response_idx, response in enumerate(responses): 71 | # filename = os.path.join(tmp_dir, f"{idx}_{response.image_type}_{response_idx}") 72 | 73 | # if response.pixels_as_float: 74 | # print("Type %d, size %d" % (response.image_type, len(response.image_data_float))) 75 | # airsim.write_pfm(os.path.normpath(filename + '.pfm'), airsim.get_pfm_array(response)) 76 | # elif response.compress: #png format 77 | # print("Type %d, size %d" % (response.image_type, len(response.image_data_uint8))) 78 | # airsim.write_file(os.path.normpath(filename + '.png'), response.image_data_uint8) 79 | # else: #uncompressed array 80 | # print("Type %d, size %d" % (response.image_type, len(response.image_data_uint8))) 81 | # img1d = np.fromstring(response.image_data_uint8, dtype=np.uint8) # get numpy array 82 | # img_rgb = img1d.reshape(response.height, response.width, 3) # reshape array to 3 channel image array H X W X 3 83 | # cv2.imwrite(os.path.normpath(filename + '.png'), img_rgb) # write to png 84 | 85 | #restore to original state 86 | client.reset() 87 | 88 | client.enableApiControl(False) 89 | -------------------------------------------------------------------------------- /Airsim/test_model.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import numpy as np 3 | import glob 4 | import os 5 | import cv2 6 | import airsim 7 | import matplotlib.pyplot as plt 8 | import tensorflow as tf 9 | 10 | from utils.functions import quaternion_to_euler, convergent_points, find_route_index 11 | from utils.normalizers import normalize_points 12 | from utils.route import Route 13 | 14 | MODEL_PATH = 'saved_models/model-59.h5' 15 | ROUTE_PATH = 'r1.csv' 16 | N = 10 17 | RANGE = 50 18 | INDX = 0 19 | 20 | 21 | ###### Load Route ###### 22 | route = Route(ROUTE_PATH, N) 23 | route_speeds = route.get_speeds() 24 | route_coordinates = route.get_coordinates() 25 | print("Route loaded") 26 | 27 | 28 | ##### Load Model ###### 29 | print('Using model {0} for testing.'.format(MODEL_PATH)) 30 | model = tf.keras.models.load_model(MODEL_PATH) 31 | print("Model loaded") 32 | 33 | 34 | print("press key") 35 | cv2.waitKey() 36 | 37 | client = airsim.CarClient() 38 | client.confirmConnection() 39 | client.enableApiControl(True) 40 | car_controls = airsim.CarControls() 41 | print('Connection established!') 42 | ##### initialize controls ##### 43 | car_controls.steering = 0 44 | car_controls.throttle = 0 45 | car_controls.brake = 0 46 | 47 | print("press key") 48 | cv2.waitKey() 49 | 50 | 51 | image_buf = np.zeros((1, 70, 254, 3)) 52 | control_buf = np.zeros((1, 4)) 53 | points_buf = np.zeros((1, 20)) 54 | 55 | 56 | def get_image(): 57 | image_response = client.simGetImages([airsim.ImageRequest(0, airsim.ImageType.Scene, False, False)])[0] 58 | image1d = np.fromstring(image_response.image_data_uint8, dtype=np.uint8) 59 | image_rgba = image1d.reshape(image_response.height, image_response.width, 3) 60 | return image_rgba[60:130, 1:255, :3].astype(float) / 255.0 61 | 62 | 63 | while (True): 64 | car_state = client.getCarState() 65 | 66 | ##### Control values ##### 67 | control_buf[0] = np.array([car_state.speed, car_controls.throttle, car_controls.steering, car_controls.brake]) 68 | 69 | qw = car_state.kinematics_estimated.orientation.w_val 70 | qx = car_state.kinematics_estimated.orientation.x_val 71 | qy = car_state.kinematics_estimated.orientation.y_val 72 | qz = car_state.kinematics_estimated.orientation.z_val 73 | 74 | x = car_state.kinematics_estimated.position.x_val 75 | y = car_state.kinematics_estimated.position.y_val 76 | 77 | pt = np.array([car_state.kinematics_estimated.position.x_val, car_state.kinematics_estimated.position.y_val]) 78 | idx_route = find_route_index(pt, route_coordinates, INDX) 79 | INDX = idx_route 80 | selected_route = route_coordinates[idx_route:idx_route+N+1] 81 | conv_pts = convergent_points(pt, selected_route) 82 | _, _, yaw = quaternion_to_euler(qx, qy, qz, qw) 83 | points = normalize_points(conv_pts, route_speeds[idx_route:idx_route+N+1], yaw) 84 | 85 | ##### Points values ##### 86 | points_buf[0] = points.ravel() 87 | 88 | ##### Image values ##### 89 | image_buf[0] = get_image() 90 | 91 | 92 | model_output = model.predict([image_buf, control_buf, points_buf]) 93 | car_controls.throttle = float(model_output[0,0]) 94 | car_controls.steering = float((model_output[0,1] - 0.5) * 2.5) 95 | car_controls.brake = float(0) 96 | 97 | print('Throttle = {0}, Steering = {1}, Brake = {2}'.format(car_controls.throttle, car_controls.steering, float(model_output[0,2]))) 98 | 99 | client.setCarControls(car_controls) -------------------------------------------------------------------------------- /Airsim/utils/__index__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TroddenSpade/Flexible-Conditional-Imitation-Learning/7ce40ffeac69f3ed09afa3523ef32adb91ad5355/Airsim/utils/__index__.py -------------------------------------------------------------------------------- /Airsim/utils/functions.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def quaternion_to_euler(q_x, q_y, q_z, q_w): 4 | # t0 = +2.0 * (q_w * q_x + q_y * q_z) 5 | # t1 = +1.0 - 2.0 * (q_x * q_x + q_y * q_y) 6 | # roll_x = np.arctan2(t0, t1) 7 | 8 | # t2 = +2.0 * (q_w * q_y - q_z * q_x) 9 | # np.clip(t2, -1.0, +1.0) 10 | # pitch_y = np.arcsin(t2) 11 | 12 | t3 = +2.0 * (q_w * q_z + q_x * q_y) 13 | t4 = +1.0 - 2.0 * (q_y * q_y + q_z * q_z) 14 | yaw_z = np.arctan2(t3, t4) 15 | 16 | return None, None, yaw_z 17 | 18 | 19 | def nearest(point, set_of_points): 20 | diff = np.square(set_of_points - point) 21 | diff = diff.sum(axis=1) 22 | return np.argmin(diff) 23 | 24 | 25 | def find_route_index(pt, route, idx, range=50): 26 | idx_ = nearest(pt, route[idx:idx+range]) 27 | idx = idx_ + idx 28 | return idx 29 | 30 | 31 | def convergent_points(pt, selcted_route): 32 | l = len(selcted_route) 33 | v = pt - selcted_route[0] 34 | r = np.linspace((1,1), (0,0), l) 35 | v = r * v 36 | s = selcted_route + v 37 | return s -------------------------------------------------------------------------------- /Airsim/utils/normalizers.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def normalize_points(set_of_points, speeds, z_rotate): 4 | s = set_of_points - set_of_points[0] 5 | diffs = s[1:] - s[:-1] 6 | norm_speed = (speeds[1:] + speeds[:-1])/2 7 | norm_diffs = diffs / np.expand_dims(norm_speed, axis=-1) 8 | s = np.cumsum(norm_diffs, axis=0) 9 | R = np.array([[np.cos(z_rotate), -np.sin(z_rotate)], 10 | [np.sin(z_rotate), np.cos(z_rotate)]]) 11 | s = s @ R 12 | return s 13 | 14 | -------------------------------------------------------------------------------- /Airsim/utils/route.py: -------------------------------------------------------------------------------- 1 | import os 2 | import pandas as pd 3 | import numpy as np 4 | 5 | class Route: 6 | def __init__(self, file_name, N) -> None: 7 | path = os.path.join('.', 'routes', file_name) 8 | self.samples = pd.read_csv(path) 9 | self.N = N 10 | 11 | def get_speeds(self): 12 | return self.samples['Speed'].to_numpy() 13 | 14 | def get_coordinates(self): 15 | xs = self.samples["POS_X"].to_numpy() 16 | ys = self.samples["POS_Y"].to_numpy() 17 | xs = np.append(xs, np.repeat(xs[-1], self.N)) 18 | ys = np.append(ys, np.repeat(ys[-1], self.N)) 19 | return np.vstack((xs,ys)).T -------------------------------------------------------------------------------- /Carla/examples/blueprint.py: -------------------------------------------------------------------------------- 1 | import carla 2 | 3 | # Creating a client 4 | client = carla.Client("127.0.0.1", 2000) 5 | client.set_timeout(10.0) 6 | client.reload_world() 7 | world = client.get_world() 8 | world = client.load_world('Town01') 9 | 10 | blueprints = [bp for bp in world.get_blueprint_library().filter('*')] 11 | for blueprint in blueprints: 12 | print(blueprint.id) 13 | for attr in blueprint: 14 | print(' - {}'.format(attr)) -------------------------------------------------------------------------------- /Carla/examples/camera.py: -------------------------------------------------------------------------------- 1 | import random 2 | import carla 3 | 4 | client = carla.Client("127.0.0.1", 2000) 5 | client.set_timeout(10.0) 6 | client.reload_world() 7 | 8 | world = client.load_world('Town01') 9 | 10 | ego_bp = world.get_blueprint_library().find('vehicle.tesla.model3') 11 | ego_bp.set_attribute('role_name','ego') 12 | print('\nEgo role_name is set') 13 | ego_color = random.choice(ego_bp.get_attribute('color').recommended_values) 14 | ego_bp.set_attribute('color',ego_color) 15 | print('\nEgo color is set') 16 | 17 | spawn_points = world.get_map().get_spawn_points() 18 | number_of_spawn_points = len(spawn_points) 19 | 20 | if 0 < number_of_spawn_points: 21 | random.shuffle(spawn_points) 22 | ego_transform = spawn_points[0] 23 | ego_vehicle = world.spawn_actor(ego_bp,ego_transform) 24 | print('\nEgo is spawned') 25 | else: 26 | logging.warning('Could not found any spawn points') 27 | 28 | 29 | # -------------- 30 | # Spawn attached RGB camera 31 | # -------------- 32 | cam_bp = None 33 | cam_bp = world.get_blueprint_library().find('sensor.camera.rgb') 34 | cam_bp.set_attribute("image_size_x",str(200)) 35 | cam_bp.set_attribute("image_size_y",str(90)) 36 | cam_bp.set_attribute("fov",str(105)) 37 | cam_location = carla.Location(2,0,1) 38 | cam_rotation = carla.Rotation(0,0,0) 39 | cam_transform = carla.Transform(cam_location,cam_rotation) 40 | ego_cam = world.spawn_actor(cam_bp,cam_transform,attach_to=ego_vehicle, attachment_type=carla.AttachmentType.Rigid) 41 | ego_cam.listen(lambda image: image.save_to_disk('./output/%.6d.jpg' % image.frame)) 42 | 43 | ego_vehicle.set_autopilot(True) 44 | 45 | world_snapshot = world.wait_for_tick() 46 | 47 | 48 | if ego_vehicle is not None: 49 | if ego_cam is not None: 50 | ego_cam.stop() 51 | ego_cam.destroy() 52 | ego_vehicle.destroy() 53 | 54 | 55 | print("Data retrieval finished") 56 | print(directory) -------------------------------------------------------------------------------- /Carla/examples/carla-data.py: -------------------------------------------------------------------------------- 1 | #Dependencies 2 | import glob 3 | import os 4 | import sys 5 | import time 6 | import numpy as np 7 | import carla 8 | import logging 9 | import random 10 | from datetime import datetime 11 | from IPython.display import display, clear_output 12 | 13 | 14 | vehicle = None 15 | cam = None 16 | 17 | #enable logging 18 | logging.basicConfig(format='%(levelname)s: %(message)s', level=logging.INFO) 19 | 20 | # Creating a client 21 | client = carla.Client("127.0.0.1", 2000) 22 | client.set_timeout(10.0) 23 | client.reload_world() 24 | for mapName in client.get_available_maps(): 25 | print(mapName) 26 | world = client.get_world() 27 | 28 | #Create Folder to store data 29 | today = datetime.now() 30 | if today.hour < 10: 31 | h = "0"+ str(today.hour) 32 | else: 33 | h = str(today.hour) 34 | if today.minute < 10: 35 | m = "0"+str(today.minute) 36 | else: 37 | m = str(today.minute) 38 | directory = "./TESTDATA/" + today.strftime('%Y%m%d_')+ h + m + "_npy" 39 | 40 | print(directory) 41 | 42 | try: 43 | os.makedirs(directory) 44 | except: 45 | print("Directory already exists") 46 | 47 | try: 48 | inputs_file = open(directory + "/inputs.npy","ba+") 49 | outputs_file = open(directory + "/outputs.npy","ba+") 50 | except: 51 | print("Files could not be opened") 52 | 53 | #Spawn vehicle 54 | #Get the blueprint concerning a tesla model 3 car 55 | bp = world.get_blueprint_library().find('vehicle.tesla.model3') 56 | #we attribute the role name brax to our blueprint 57 | bp.set_attribute('role_name','brax') 58 | #get a random color 59 | color = random.choice(bp.get_attribute('color').recommended_values) 60 | #put the selected color on our blueprint 61 | bp.set_attribute('color',color) 62 | 63 | #get all spawn points 64 | spawn_points = world.get_map().get_spawn_points() 65 | number_of_spawn_points = len(spawn_points) 66 | 67 | #select a random spawn point 68 | if 0 < number_of_spawn_points: 69 | random.shuffle(spawn_points) 70 | transform = spawn_points[0] 71 | #spawn our vehicle ! 72 | vehicle = world.spawn_actor(bp,transform) 73 | print('\nVehicle spawned') 74 | else: 75 | #no spawn points 76 | logging.warning('Could not found any spawn points') 77 | 78 | #Adding a RGB camera sensor 79 | WIDTH = 200 80 | HEIGHT = 88 81 | cam_bp = None 82 | #Get blueprint of a camera 83 | cam_bp = world.get_blueprint_library().find('sensor.camera.rgb') 84 | #Set attributes 85 | cam_bp.set_attribute("image_size_x",str(WIDTH)) 86 | cam_bp.set_attribute("image_size_y",str(HEIGHT)) 87 | cam_bp.set_attribute("fov",str(105)) 88 | #Location to attach the camera on the car 89 | cam_location = carla.Location(2,0,1) 90 | cam_rotation = carla.Rotation(0,0,0) 91 | cam_transform = carla.Transform(cam_location,cam_rotation) 92 | #Spawn the camera and attach it to our vehicle 93 | cam = world.spawn_actor(cam_bp,cam_transform,attach_to=vehicle, attachment_type=carla.AttachmentType.Rigid) 94 | 95 | #Function to convert image to a numpy array 96 | def process_image(image): 97 | #Get raw image in 8bit format 98 | raw_image = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) 99 | #Reshape image to RGBA 100 | raw_image = np.reshape(raw_image, (image.height, image.width, 4)) 101 | #Taking only RGB 102 | processed_image = raw_image[:, :, :3]/255 103 | return processed_image 104 | 105 | #Save required data 106 | def save_image(carla_image): 107 | image = process_image(carla_image) 108 | control = vehicle.get_control() 109 | location = vehicle.get_location() 110 | transform = vehicle.get_transform() 111 | velocity = vehicle.get_velocity() 112 | print(control, location, transform, velocity) 113 | ''' 114 | bounding_box 115 | apply_control(vehicle_control) 116 | get_control() 117 | set_autopilot(enabled=True) 118 | get_speed_limit() 119 | get_traffic_light_state() 120 | is_at_traffic_light() 121 | get_traffic_light() 122 | ''' 123 | data = [control.steer, control.throttle, control.brake] 124 | np.save(inputs_file, image) 125 | np.save(outputs_file, data) 126 | 127 | #enable auto pilot 128 | vehicle.set_autopilot(True) 129 | #Attach event listeners 130 | cam.listen(save_image) 131 | world_snapshot = world.wait_for_tick() 132 | 133 | 134 | try: 135 | i = 0 136 | while i < 3000: 137 | world_snapshot = world.wait_for_tick() 138 | clear_output(wait=True) 139 | display(f"{str(i)} frames saved") 140 | i += 1 141 | except Exception as inst: 142 | print('\nSimulation error.') 143 | print(inst) 144 | print(inst.args) 145 | 146 | if vehicle is not None: 147 | if cam is not None: 148 | cam.stop() 149 | cam.destroy() 150 | vehicle.destroy() 151 | inputs_file.close() 152 | outputs_file.close() 153 | print("Data retrieval finished") 154 | print(directory) -------------------------------------------------------------------------------- /Carla/examples/generate_traffic.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2021 Computer Vision Center (CVC) at the Universitat Autonoma de 4 | # Barcelona (UAB). 5 | # 6 | # This work is licensed under the terms of the MIT license. 7 | # For a copy, see . 8 | 9 | """Example script to generate traffic in the simulation""" 10 | 11 | import glob 12 | import os 13 | import sys 14 | import time 15 | 16 | # try: 17 | # sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % ( 18 | # sys.version_info.major, 19 | # sys.version_info.minor, 20 | # 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) 21 | # except IndexError: 22 | # pass 23 | 24 | import carla 25 | 26 | import logging 27 | from numpy import random 28 | 29 | def get_actor_blueprints(world, filter, generation): 30 | bps = world.get_blueprint_library().filter(filter) 31 | 32 | if generation.lower() == "all": 33 | return bps 34 | 35 | # If the filter returns only one bp, we assume that this one needed 36 | # and therefore, we ignore the generation 37 | if len(bps) == 1: 38 | return bps 39 | 40 | try: 41 | int_generation = int(generation) 42 | # Check if generation is in available generations 43 | if int_generation in [1, 2]: 44 | bps = [x for x in bps if int(x.get_attribute('generation')) == int_generation] 45 | return bps 46 | else: 47 | print(" Warning! Actor Generation is not valid. No actor will be spawned.") 48 | return [] 49 | except: 50 | print(" Warning! Actor Generation is not valid. No actor will be spawned.") 51 | return [] 52 | 53 | def generate_traffic(client, world): 54 | 55 | logging.basicConfig(format='%(levelname)s: %(message)s', level=logging.INFO) 56 | 57 | vehicles_list = [] 58 | walkers_list = [] 59 | all_id = [] 60 | 61 | synchronous_master = False 62 | random.seed(int(time.time())) 63 | 64 | try: 65 | 66 | traffic_manager = client.get_trafficmanager(8000) 67 | traffic_manager.set_global_distance_to_leading_vehicle(2.5) 68 | traffic_manager.set_hybrid_physics_mode(True) 69 | traffic_manager.set_hybrid_physics_radius(70.0) 70 | 71 | # settings = world.get_settings() 72 | # world.apply_settings(settings) 73 | 74 | print("You are currently in asynchronous mode. If this is a traffic simulation, \ 75 | you could experience some issues. If it's not working correctly, switch to synchronous \ 76 | mode by using traffic_manager.set_synchronous_mode(True)") 77 | 78 | 79 | blueprintsWalkers = get_actor_blueprints(world, 'walker.pedestrian.*', '2') 80 | 81 | blueprints = get_actor_blueprints(world, 'vehicle.*', 'All') 82 | blueprints = [x for x in blueprints if int(x.get_attribute('number_of_wheels')) == 4] 83 | blueprints = [x for x in blueprints if not x.id.endswith('microlino')] 84 | blueprints = [x for x in blueprints if not x.id.endswith('carlacola')] 85 | blueprints = [x for x in blueprints if not x.id.endswith('cybertruck')] 86 | blueprints = [x for x in blueprints if not x.id.endswith('t2')] 87 | blueprints = [x for x in blueprints if not x.id.endswith('sprinter')] 88 | blueprints = [x for x in blueprints if not x.id.endswith('firetruck')] 89 | blueprints = [x for x in blueprints if not x.id.endswith('ambulance')] 90 | 91 | blueprints = sorted(blueprints, key=lambda bp: bp.id) 92 | 93 | spawn_points = world.get_map().get_spawn_points() 94 | number_of_spawn_points = len(spawn_points) 95 | number_of_vehicles = 30 96 | 97 | if number_of_vehicles < number_of_spawn_points: 98 | random.shuffle(spawn_points) 99 | elif number_of_vehicles > number_of_spawn_points: 100 | msg = 'requested %d vehicles, but could only find %d spawn points' 101 | logging.warning(msg, number_of_vehicles, number_of_spawn_points) 102 | number_of_vehicles = number_of_spawn_points 103 | 104 | # @todo cannot import these directly. 105 | SpawnActor = carla.command.SpawnActor 106 | SetAutopilot = carla.command.SetAutopilot 107 | FutureActor = carla.command.FutureActor 108 | 109 | # -------------- 110 | # Spawn vehicles 111 | # -------------- 112 | batch = [] 113 | for n, transform in enumerate(spawn_points): 114 | if n >= number_of_vehicles: 115 | break 116 | blueprint = random.choice(blueprints) 117 | if blueprint.has_attribute('color'): 118 | color = random.choice(blueprint.get_attribute('color').recommended_values) 119 | blueprint.set_attribute('color', color) 120 | if blueprint.has_attribute('driver_id'): 121 | driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values) 122 | blueprint.set_attribute('driver_id', driver_id) 123 | blueprint.set_attribute('role_name', 'autopilot') 124 | 125 | # spawn the cars and set their autopilot and light state all together 126 | batch.append(SpawnActor(blueprint, transform) 127 | .then(SetAutopilot(FutureActor, True, traffic_manager.get_port()))) 128 | 129 | for response in client.apply_batch_sync(batch, synchronous_master): 130 | if response.error: 131 | logging.error(response.error) 132 | else: 133 | vehicles_list.append(response.actor_id) 134 | 135 | # ------------- 136 | # Spawn Walkers 137 | # ------------- 138 | # some settings 139 | percentagePedestriansRunning = 0.0 # how many pedestrians will run 140 | percentagePedestriansCrossing = 0.5 # how many pedestrians will walk through the road 141 | world.set_pedestrians_seed(0) 142 | random.seed(0) 143 | # 1. take all the random locations to spawn 144 | number_of_walkers = 10 145 | spawn_points = [] 146 | for i in range(number_of_walkers): 147 | spawn_point = carla.Transform() 148 | loc = world.get_random_location_from_navigation() 149 | if (loc != None): 150 | spawn_point.location = loc 151 | spawn_points.append(spawn_point) 152 | # 2. we spawn the walker object 153 | batch = [] 154 | walker_speed = [] 155 | for spawn_point in spawn_points: 156 | walker_bp = random.choice(blueprintsWalkers) 157 | # set as not invincible 158 | if walker_bp.has_attribute('is_invincible'): 159 | walker_bp.set_attribute('is_invincible', 'false') 160 | # set the max speed 161 | if walker_bp.has_attribute('speed'): 162 | if (random.random() > percentagePedestriansRunning): 163 | # walking 164 | walker_speed.append(walker_bp.get_attribute('speed').recommended_values[1]) 165 | else: 166 | # running 167 | walker_speed.append(walker_bp.get_attribute('speed').recommended_values[2]) 168 | else: 169 | print("Walker has no speed") 170 | walker_speed.append(0.0) 171 | batch.append(SpawnActor(walker_bp, spawn_point)) 172 | results = client.apply_batch_sync(batch, True) 173 | walker_speed2 = [] 174 | for i in range(len(results)): 175 | if results[i].error: 176 | logging.error(results[i].error) 177 | else: 178 | walkers_list.append({"id": results[i].actor_id}) 179 | walker_speed2.append(walker_speed[i]) 180 | walker_speed = walker_speed2 181 | # 3. we spawn the walker controller 182 | batch = [] 183 | walker_controller_bp = world.get_blueprint_library().find('controller.ai.walker') 184 | for i in range(len(walkers_list)): 185 | batch.append(SpawnActor(walker_controller_bp, carla.Transform(), walkers_list[i]["id"])) 186 | results = client.apply_batch_sync(batch, True) 187 | for i in range(len(results)): 188 | if results[i].error: 189 | logging.error(results[i].error) 190 | else: 191 | walkers_list[i]["con"] = results[i].actor_id 192 | # 4. we put together the walkers and controllers id to get the objects from their id 193 | for i in range(len(walkers_list)): 194 | all_id.append(walkers_list[i]["con"]) 195 | all_id.append(walkers_list[i]["id"]) 196 | all_actors = world.get_actors(all_id) 197 | 198 | # wait for a tick to ensure client receives the last transform of the walkers we have just created 199 | world.wait_for_tick() 200 | 201 | 202 | # 5. initialize each controller and set target to walk to (list is [controler, actor, controller, actor ...]) 203 | # set how many pedestrians can cross the road 204 | world.set_pedestrians_cross_factor(percentagePedestriansCrossing) 205 | for i in range(0, len(all_id), 2): 206 | # start walker 207 | all_actors[i].start() 208 | # set walk to random point 209 | all_actors[i].go_to_location(world.get_random_location_from_navigation()) 210 | # max speed 211 | all_actors[i].set_max_speed(float(walker_speed[int(i/2)])) 212 | 213 | print('spawned %d vehicles and %d walkers, press Ctrl+C to exit.' % (len(vehicles_list), len(walkers_list))) 214 | 215 | # Example of how to use Traffic Manager parameters 216 | traffic_manager.global_percentage_speed_difference(30.0) 217 | 218 | 219 | finally: 220 | 221 | print('\ndestroying %d vehicles' % len(vehicles_list)) 222 | client.apply_batch([carla.command.DestroyActor(x) for x in vehicles_list]) 223 | 224 | # stop walker controllers (list is [controller, actor, controller, actor ...]) 225 | for i in range(0, len(all_id), 2): 226 | all_actors[i].stop() 227 | 228 | print('\ndestroying %d walkers' % len(walkers_list)) 229 | client.apply_batch([carla.command.DestroyActor(x) for x in all_id]) 230 | 231 | time.sleep(0.5) 232 | 233 | 234 | if __name__ == '__main__': 235 | try: 236 | generate_traffic() 237 | except KeyboardInterrupt: 238 | pass 239 | finally: 240 | print('\ndone.') 241 | -------------------------------------------------------------------------------- /Carla/examples/maps.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import carla 3 | 4 | client = carla.Client("127.0.0.1", 2000) 5 | client.set_timeout(10.0) 6 | client.reload_world() 7 | print(client.get_available_maps()) 8 | 9 | world = client.load_world('Town01') -------------------------------------------------------------------------------- /Carla/examples/retrive_data.py: -------------------------------------------------------------------------------- 1 | #Dependencies 2 | from email import header 3 | import glob 4 | import os 5 | import sys 6 | import time 7 | import numpy as np 8 | import carla 9 | import logging 10 | import random 11 | import pandas as pd 12 | from datetime import datetime 13 | 14 | from agents.navigation.global_route_planner import GlobalRoutePlanner 15 | 16 | 17 | TOWN_NAME = 'Town01' 18 | DIRECTORY = 'Data' 19 | IMAGE_WIDTH = 88 20 | IMAGE_HEIGHT = 200 21 | RECORD_LENGTH = 1000 22 | NUMBER_OF_VEHICLES = 0 23 | NUMBER_OF_WALKERS = 0 24 | 25 | 26 | # def get_actor_blueprints(world, filter, generation): 27 | # bps = world.get_blueprint_library().filter(filter) 28 | 29 | # if generation.lower() == "all": 30 | # return bps 31 | 32 | # # If the filter returns only one bp, we assume that this one needed 33 | # # and therefore, we ignore the generation 34 | # if len(bps) == 1: 35 | # return bps 36 | 37 | # try: 38 | # int_generation = int(generation) 39 | # # Check if generation is in available generations 40 | # if int_generation in [1, 2]: 41 | # bps = [x for x in bps if int(x.get_attribute('generation')) == int_generation] 42 | # return bps 43 | # else: 44 | # print(" Warning! Actor Generation is not valid. No actor will be spawned.") 45 | # return [] 46 | # except: 47 | # print(" Warning! Actor Generation is not valid. No actor will be spawned.") 48 | # return [] 49 | 50 | 51 | 52 | # datetime object containing current date and time 53 | now = datetime.now() 54 | # dd/mm/YY H:M:S 55 | dt_string = now.strftime("%Y-%m-%d_%H-%M-%S") 56 | print('\nStarted at =', dt_string) 57 | 58 | rec_dir = os.path.join(DIRECTORY, dt_string) 59 | image_dir = os.path.join(rec_dir, 'images') 60 | try: 61 | os.makedirs(image_dir) 62 | print("\n"+ rec_dir + ' created') 63 | except Exception as inst: 64 | print("\nError: ") 65 | print(inst) 66 | 67 | 68 | client = carla.Client("127.0.0.1", 2000) 69 | client.set_timeout(10.0) 70 | client.reload_world() 71 | 72 | world = client.load_world(TOWN_NAME) 73 | print('\n'+ TOWN_NAME + ' Loaded.') 74 | 75 | 76 | ego_bp = world.get_blueprint_library().find('vehicle.tesla.model3') 77 | ego_bp.set_attribute('role_name','ego') 78 | ego_color = random.choice(ego_bp.get_attribute('color').recommended_values) 79 | ego_bp.set_attribute('color',ego_color) 80 | 81 | spawn_points = world.get_map().get_spawn_points() 82 | number_of_spawn_points = len(spawn_points) 83 | 84 | if 0 < number_of_spawn_points: 85 | random.shuffle(spawn_points) 86 | ego_transform = spawn_points[0] 87 | ego_vehicle = world.spawn_actor(ego_bp,ego_transform) 88 | spawn_points.pop(0) 89 | number_of_spawn_points -= 1 90 | print('\nVehicle is spawned') 91 | else: 92 | print('\nCould not found any spawn points!') 93 | 94 | world.wait_for_tick() 95 | 96 | 97 | logging.basicConfig(format='%(levelname)s: %(message)s', level=logging.INFO) 98 | 99 | # vehicles_list = [] 100 | # walkers_list = [] 101 | # all_id = [] 102 | 103 | # synchronous_master = False 104 | # random.seed(int(time.time())) 105 | 106 | # traffic_manager = client.get_trafficmanager(8000) 107 | # traffic_manager.set_global_distance_to_leading_vehicle(2.5) 108 | # traffic_manager.set_hybrid_physics_mode(True) 109 | # traffic_manager.set_hybrid_physics_radius(70.0) 110 | # traffic_manager.set_synchronous_mode(True) 111 | 112 | # # settings = world.get_settings() 113 | # # world.apply_settings(settings) 114 | 115 | 116 | # blueprintsWalkers = get_actor_blueprints(world, 'walker.pedestrian.*', '2') 117 | 118 | # blueprints = get_actor_blueprints(world, 'vehicle.*', 'All') 119 | # blueprints = [x for x in blueprints if int(x.get_attribute('number_of_wheels')) == 4] 120 | # blueprints = [x for x in blueprints if not x.id.endswith('microlino')] 121 | # blueprints = [x for x in blueprints if not x.id.endswith('carlacola')] 122 | # blueprints = [x for x in blueprints if not x.id.endswith('cybertruck')] 123 | # blueprints = [x for x in blueprints if not x.id.endswith('t2')] 124 | # blueprints = [x for x in blueprints if not x.id.endswith('sprinter')] 125 | # blueprints = [x for x in blueprints if not x.id.endswith('firetruck')] 126 | # blueprints = [x for x in blueprints if not x.id.endswith('ambulance')] 127 | 128 | # blueprints = sorted(blueprints, key=lambda bp: bp.id) 129 | 130 | # if NUMBER_OF_VEHICLES < number_of_spawn_points: 131 | # random.shuffle(spawn_points) 132 | # elif NUMBER_OF_VEHICLES > number_of_spawn_points: 133 | # msg = 'requested %d vehicles, but could only find %d spawn points' 134 | # logging.warning(msg, NUMBER_OF_VEHICLES, number_of_spawn_points) 135 | # NUMBER_OF_VEHICLES = number_of_spawn_points 136 | 137 | # # @todo cannot import these directly. 138 | # SpawnActor = carla.command.SpawnActor 139 | # SetAutopilot = carla.command.SetAutopilot 140 | # FutureActor = carla.command.FutureActor 141 | 142 | # # -------------- 143 | # # Spawn vehicles 144 | # # -------------- 145 | # batch = [] 146 | # for n, transform in enumerate(spawn_points): 147 | # if n >= NUMBER_OF_VEHICLES: 148 | # break 149 | # blueprint = random.choice(blueprints) 150 | # if blueprint.has_attribute('color'): 151 | # color = random.choice(blueprint.get_attribute('color').recommended_values) 152 | # blueprint.set_attribute('color', color) 153 | # if blueprint.has_attribute('driver_id'): 154 | # driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values) 155 | # blueprint.set_attribute('driver_id', driver_id) 156 | # blueprint.set_attribute('role_name', 'autopilot') 157 | 158 | # # spawn the cars and set their autopilot and light state all together 159 | # batch.append(SpawnActor(blueprint, transform) 160 | # .then(SetAutopilot(FutureActor, True, traffic_manager.get_port()))) 161 | 162 | # for response in client.apply_batch_sync(batch, synchronous_master): 163 | # if response.error: 164 | # logging.error(response.error) 165 | # else: 166 | # vehicles_list.append(response.actor_id) 167 | 168 | # # ------------- 169 | # # Spawn Walkers 170 | # # ------------- 171 | # # some settings 172 | # percentagePedestriansRunning = 0.0 # how many pedestrians will run 173 | # percentagePedestriansCrossing = 0.5 # how many pedestrians will walk through the road 174 | # world.set_pedestrians_seed(0) 175 | # random.seed(0) 176 | # # 1. take all the random locations to spawn 177 | # spawn_points = [] 178 | # for i in range(NUMBER_OF_WALKERS): 179 | # spawn_point = carla.Transform() 180 | # loc = world.get_random_location_from_navigation() 181 | # if (loc != None): 182 | # spawn_point.location = loc 183 | # spawn_points.append(spawn_point) 184 | # # 2. we spawn the walker object 185 | # batch = [] 186 | # walker_speed = [] 187 | # for spawn_point in spawn_points: 188 | # walker_bp = random.choice(blueprintsWalkers) 189 | # # set as not invincible 190 | # if walker_bp.has_attribute('is_invincible'): 191 | # walker_bp.set_attribute('is_invincible', 'false') 192 | # # set the max speed 193 | # if walker_bp.has_attribute('speed'): 194 | # if (random.random() > percentagePedestriansRunning): 195 | # # walking 196 | # walker_speed.append(walker_bp.get_attribute('speed').recommended_values[1]) 197 | # else: 198 | # # running 199 | # walker_speed.append(walker_bp.get_attribute('speed').recommended_values[2]) 200 | # else: 201 | # print("Walker has no speed") 202 | # walker_speed.append(0.0) 203 | # batch.append(SpawnActor(walker_bp, spawn_point)) 204 | # results = client.apply_batch_sync(batch, True) 205 | # walker_speed2 = [] 206 | # for i in range(len(results)): 207 | # if results[i].error: 208 | # logging.error(results[i].error) 209 | # else: 210 | # walkers_list.append({"id": results[i].actor_id}) 211 | # walker_speed2.append(walker_speed[i]) 212 | # walker_speed = walker_speed2 213 | # # 3. we spawn the walker controller 214 | # batch = [] 215 | # walker_controller_bp = world.get_blueprint_library().find('controller.ai.walker') 216 | # for i in range(len(walkers_list)): 217 | # batch.append(SpawnActor(walker_controller_bp, carla.Transform(), walkers_list[i]["id"])) 218 | # results = client.apply_batch_sync(batch, True) 219 | # for i in range(len(results)): 220 | # if results[i].error: 221 | # logging.error(results[i].error) 222 | # else: 223 | # walkers_list[i]["con"] = results[i].actor_id 224 | # # 4. we put together the walkers and controllers id to get the objects from their id 225 | # for i in range(len(walkers_list)): 226 | # all_id.append(walkers_list[i]["con"]) 227 | # all_id.append(walkers_list[i]["id"]) 228 | # all_actors = world.get_actors(all_id) 229 | 230 | # # wait for a tick to ensure client receives the last transform of the walkers we have just created 231 | # world.wait_for_tick() 232 | 233 | 234 | # # 5. initialize each controller and set target to walk to (list is [controler, actor, controller, actor ...]) 235 | # # set how many pedestrians can cross the road 236 | # world.set_pedestrians_cross_factor(percentagePedestriansCrossing) 237 | # for i in range(0, len(all_id), 2): 238 | # # start walker 239 | # all_actors[i].start() 240 | # # set walk to random point 241 | # all_actors[i].go_to_location(world.get_random_location_from_navigation()) 242 | # # max speed 243 | # all_actors[i].set_max_speed(float(walker_speed[int(i/2)])) 244 | 245 | # print('spawned %d vehicles and %d walkers, press Ctrl+C to exit.' % (len(vehicles_list), len(walkers_list))) 246 | 247 | # # Example of how to use Traffic Manager parameters 248 | # traffic_manager.global_percentage_speed_difference(30.0) 249 | 250 | 251 | # -------------- 252 | # Spawn attached RGB camera 253 | # -------------- 254 | 255 | car_locations = [] 256 | 257 | data = { 258 | 'image_name': [], 259 | 'steer':[], 260 | 'throttle':[], 261 | 'brake':[], 262 | 'speed':[], 263 | 'x':[], 264 | 'y':[], 265 | 'z':[], 266 | 'yaw':[], 267 | 'speed_limit':[], 268 | 'is_traffic_light':[], 269 | 'traffic_light_state':[], 270 | 271 | } 272 | 273 | 274 | def data_handler(image): 275 | img_name = dt_string + '_' + str(image.frame) + '.jpg' 276 | image.save_to_disk(os.path.join(image_dir, img_name)) 277 | 278 | control = ego_vehicle.get_control() 279 | transform = ego_vehicle.get_transform() 280 | velocity = ego_vehicle.get_velocity().length() 281 | speed_limit = ego_vehicle.get_speed_limit() 282 | light_state = ego_vehicle.get_traffic_light_state() 283 | is_traffic_light = ego_vehicle.is_at_traffic_light() 284 | # traffic_light = ego_vehicle.get_traffic_light() 285 | 286 | data['image_name'].append(img_name) 287 | data['steer'].append(control.steer) 288 | data['throttle'].append(control.throttle) 289 | data['brake'].append(control.brake) 290 | data['speed'].append(velocity) 291 | data['x'].append(transform.location.x) 292 | data['y'].append(transform.location.y) 293 | data['z'].append(transform.location.z) 294 | data['yaw'].append(transform.rotation.yaw) 295 | data['speed_limit'].append(speed_limit) 296 | data['is_traffic_light'].append(is_traffic_light) 297 | data['traffic_light_state'].append(light_state) 298 | 299 | car_locations.append(transform.location) 300 | 301 | 302 | cam_bp = None 303 | cam_bp = world.get_blueprint_library().find('sensor.camera.rgb') 304 | cam_bp.set_attribute("image_size_x",str(IMAGE_HEIGHT)) 305 | cam_bp.set_attribute("image_size_y",str(IMAGE_WIDTH)) 306 | cam_bp.set_attribute("fov",str(105)) 307 | cam_location = carla.Location(2,0,1) 308 | cam_rotation = carla.Rotation(0,0,0) 309 | cam_transform = carla.Transform(cam_location,cam_rotation) 310 | ego_cam = world.spawn_actor(cam_bp,cam_transform,attach_to=ego_vehicle, attachment_type=carla.AttachmentType.Rigid) 311 | 312 | 313 | ego_vehicle.set_autopilot(True) 314 | print('\nEgo autopilot enabled') 315 | 316 | ego_cam.listen(data_handler) 317 | world_snapshot = world.wait_for_tick() 318 | 319 | try: 320 | i = 1 321 | while i <= RECORD_LENGTH: 322 | world_snapshot = world.wait_for_tick() 323 | print(f"Frame {str(i)} saved") 324 | i += 1 325 | 326 | except Exception as inst: 327 | print('\nSimulation error:') 328 | print(inst) 329 | 330 | 331 | 332 | if ego_vehicle is not None: 333 | if ego_cam is not None: 334 | ego_cam.stop() 335 | ego_cam.destroy() 336 | ego_vehicle.destroy() 337 | 338 | time.sleep(0.5) 339 | 340 | # print('\ndestroying %d vehicles' % len(vehicles_list)) 341 | # client.apply_batch([carla.command.DestroyActor(x) for x in vehicles_list]) 342 | 343 | # # stop walker controllers (list is [controller, actor, controller, actor ...]) 344 | # for i in range(0, len(all_id), 2): 345 | # all_actors[i].stop() 346 | 347 | # print('\ndestroying %d walkers' % len(walkers_list)) 348 | # client.apply_batch([carla.command.DestroyActor(x) for x in all_id]) 349 | 350 | # time.sleep(0.5) 351 | 352 | # print("\nSimulation finished") 353 | 354 | 355 | 356 | 357 | print("\nSetting Waypoints' Coordinates") 358 | map = world.get_map() 359 | sampling_resolution = 2 360 | grp = GlobalRoutePlanner(map, sampling_resolution) 361 | 362 | 363 | waypoints = [] 364 | waypoints_p = [] 365 | 366 | for idx, loc in enumerate(car_locations): 367 | 368 | 369 | i = idx + 1 370 | if i >= len(car_locations): 371 | break 372 | while i < len(car_locations) and car_locations[i].distance(car_locations[idx]) < 30: 373 | i += 1 374 | 375 | if i < len(car_locations): 376 | w1 = grp.trace_route(car_locations[idx], car_locations[i]) 377 | else: 378 | w1 = [] 379 | 380 | wp = [] 381 | wp_p = [] 382 | for w in w1: 383 | x, y = w[0].transform.location.x, w[0].transform.location.y 384 | wp.append([x, y]) 385 | 386 | theta = w[0].transform.rotation.yaw * np.pi/180 387 | v_p_x, v_p_y = -w[0].lane_width * np.sin(theta), w[0].lane_width * np.cos(theta) 388 | p_p = [x - v_p_x, y - v_p_y] 389 | wp_p.append(p_p) 390 | loc = carla.Location(x=p_p[0], y=p_p[1], z=0.0) 391 | 392 | print(len(w1)) 393 | waypoints.append(wp) 394 | waypoints_p.append(wp_p) 395 | 396 | 397 | 398 | print("\nData retrieval finished") 399 | print(rec_dir) 400 | for k, v in data.items(): 401 | print(k, len(v)) 402 | 403 | df = pd.DataFrame(data=data) 404 | df.to_csv(os.path.join(rec_dir, 'data.csv'), index=False) 405 | 406 | np.save(os.path.join(rec_dir, 'waypoints'), waypoints) 407 | np.save(os.path.join(rec_dir, 'waypoints_p'), waypoints_p) 408 | -------------------------------------------------------------------------------- /Carla/examples/route.py: -------------------------------------------------------------------------------- 1 | import carla 2 | import numpy as np 3 | 4 | from agents.navigation.global_route_planner import GlobalRoutePlanner 5 | 6 | client = carla.Client("localhost", 2000) 7 | client.set_timeout(10) 8 | world = client.load_world('Town01') 9 | amap = world.get_map() 10 | 11 | 12 | sampling_resolution = 2 13 | grp = GlobalRoutePlanner(amap, sampling_resolution) 14 | 15 | 16 | spawn_points = world.get_map().get_spawn_points() 17 | a = carla.Location(spawn_points[50].location) 18 | b = carla.Location(spawn_points[100].location) 19 | 20 | w1 = grp.trace_route(a, b) 21 | 22 | wp = [] 23 | wp_p = [] 24 | 25 | for w in w1: 26 | x, y = w[0].transform.location.x, w[0].transform.location.y 27 | wp.append([x, y]) 28 | world.debug.draw_string(w[0].transform.location, 'O', draw_shadow=False, 29 | color=carla.Color(r=255, g=0, b=0), life_time=120.0, persistent_lines=True) 30 | 31 | theta = w[0].transform.rotation.yaw * np.pi/180 32 | v_p, v_p = -w.lane_width * np.sin(theta), w.lane_width * np.cos(theta) 33 | p_p = [x + v_p, y + v_p] 34 | wp_p.append(p_p) 35 | loc = carla.Location(x=p_p[0], y=p_p[1], z=0.0) 36 | world.debug.draw_string(loc, 'O', draw_shadow=False, 37 | color = carla.Color(r=0, g=0, b=255), life_time=1000.0,persistent_lines=True) 38 | 39 | 40 | -------------------------------------------------------------------------------- /Carla/examples/spawnpoints.py: -------------------------------------------------------------------------------- 1 | import carla 2 | 3 | # Creating a client 4 | client = carla.Client("127.0.0.1", 2000) 5 | client.set_timeout(10.0) 6 | client.reload_world() 7 | for mapName in client.get_available_maps(): 8 | print(mapName) 9 | 10 | world = client.get_world() 11 | 12 | #get all spawn points 13 | spawn_points = world.get_map().get_spawn_points() 14 | number_of_spawn_points = len(spawn_points) 15 | 16 | print(number_of_spawn_points) 17 | 18 | print(spawn_points) -------------------------------------------------------------------------------- /Carla/examples/vehicle.py: -------------------------------------------------------------------------------- 1 | import random 2 | import carla 3 | 4 | client = carla.Client("127.0.0.1", 2000) 5 | client.set_timeout(10.0) 6 | client.reload_world() 7 | print(client.get_available_maps()) 8 | 9 | world = client.load_world('Town01') 10 | 11 | ego_bp = world.get_blueprint_library().find('vehicle.tesla.model3') 12 | ego_bp.set_attribute('role_name','ego') 13 | print('\nEgo role_name is set') 14 | ego_color = random.choice(ego_bp.get_attribute('color').recommended_values) 15 | ego_bp.set_attribute('color',ego_color) 16 | print('\nEgo color is set') 17 | 18 | spawn_points = world.get_map().get_spawn_points() 19 | number_of_spawn_points = len(spawn_points) 20 | 21 | if 0 < number_of_spawn_points: 22 | random.shuffle(spawn_points) 23 | ego_transform = spawn_points[0] 24 | ego_vehicle = world.spawn_actor(ego_bp,ego_transform) 25 | print('\nEgo is spawned') 26 | else: 27 | logging.warning('Could not found any spawn points') 28 | 29 | print(ego_vehicle) 30 | -------------------------------------------------------------------------------- /Carla/examples/waypoints-test.py: -------------------------------------------------------------------------------- 1 | import matplotlib 2 | import matplotlib.pyplot as plt 3 | import numpy as np 4 | import pickle 5 | 6 | # with open(r'C:\Users\hp\Desktop\Autonomous-Car\Carla\waypoints\pos', 'rb') as fp: 7 | # pos = pickle.load(fp) 8 | # pos = np.load(r"C:\Users\hp\Desktop\Autonomous-Car\Carla\waypoints\pos.npy", allow_pickle=True) 9 | w1 = np.load(r"D:\Users\Parsa Sam\Documents\GitHub\Autonomous-Car\Carla\waypoints\1.npy") 10 | 11 | plt.scatter(w1[:,0], w1[:,1], s=5) 12 | # plt.scatter(w2[:,0], w2[:,1], s=5) 13 | 14 | # print(pos.items()) 15 | # for k,v in pos.items(): 16 | # v = np.array(v) 17 | # plt.scatter(v[:, 0], v[:,1], s=5) 18 | 19 | plt.show() -------------------------------------------------------------------------------- /Carla/examples/waypoints.py: -------------------------------------------------------------------------------- 1 | import carla 2 | import numpy as np 3 | import pickle 4 | import matplotlib.pyplot as plt 5 | 6 | 7 | 8 | # Creating a client 9 | client = carla.Client("127.0.0.1", 2000) 10 | client.set_timeout(10.0) 11 | client.reload_world() 12 | 13 | 14 | world = client.load_world('Town01') 15 | 16 | map = world.get_map() 17 | 18 | waypoint_list = map.generate_waypoints(2.0) 19 | 20 | coor = [] 21 | roads = {} 22 | for i,w in enumerate(waypoint_list): 23 | if w.road_id not in roads: 24 | roads[w.road_id] = [] 25 | else: 26 | roads[w.road_id].append([w.transform.location.x, w.transform.location.y]) 27 | 28 | print(roads) 29 | 30 | 31 | # with open(r'C:\Users\hp\Desktop\Autonomous-Car\Carla\waypoints\w1', 'wb') as fp: 32 | # pickle.dump(coor, fp) 33 | 34 | 35 | 36 | # with open(r'C:\Users\hp\Desktop\Autonomous-Car\Carla\waypoints\pos', 'rb') as fp: 37 | # pos = pickle.load(fp) 38 | # pos = np.load(r"C:\Users\hp\Desktop\Autonomous-Car\Carla\waypoints\pos.npy", allow_pickle=True) 39 | 40 | 41 | c = 0 42 | for k,v in roads.items(): 43 | v = np.array(v) 44 | plt.scatter(v[:, 0], v[:,1], s=5) 45 | c+=1 46 | if c == 20: 47 | break 48 | 49 | plt.show() -------------------------------------------------------------------------------- /Carla/info/Frame Controls.txt: -------------------------------------------------------------------------------- 1 | Frame 1 saved 2 | VehicleControl(throttle=0.000000, steer=0.000000, brake=0.000000, hand_brake=False, reverse=False, manual_gear_shift=False, gear=0) Transform(Location(x=-1.980020, y=249.430008, z=0.001575), Rotation(pitch=0.000000, yaw=90.000046, roll=0.000000)) 0.0 30.0 Green False None 3 | Frame 2 saved 4 | VehicleControl(throttle=0.000000, steer=0.000000, brake=0.000000, hand_brake=False, reverse=False, manual_gear_shift=False, gear=0) Transform(Location(x=-1.980020, y=249.430008, z=0.001575), Rotation(pitch=0.000000, yaw=90.000046, roll=0.000000)) 0.0 30.0 Green False None 5 | Frame 3 saved 6 | VehicleControl(throttle=0.000000, steer=0.000000, brake=0.000000, hand_brake=False, reverse=False, manual_gear_shift=False, gear=0) Transform(Location(x=-1.980020, y=249.430008, z=0.001575), Rotation(pitch=0.000000, yaw=90.000046, roll=0.000000)) 0.0 30.0 Green False None 7 | Frame 4 saved 8 | VehicleControl(throttle=0.850000, steer=0.000616, brake=0.000000, hand_brake=False, reverse=False, manual_gear_shift=False, gear=0) Transform(Location(x=-1.980020, y=249.430008, z=0.001501), Rotation(pitch=0.000000, yaw=90.000046, roll=0.000000)) 0.0020086669828742743 30.0 Green False None 9 | Frame 5 saved 10 | VehicleControl(throttle=0.850000, steer=0.000616, brake=0.000000, hand_brake=False, reverse=False, manual_gear_shift=False, gear=0) Transform(Location(x=-1.980020, y=249.430008, z=0.001459), Rotation(pitch=0.000000, yaw=90.000046, roll=0.000000)) 0.0020722292829304934 30.0 Green False None 11 | Frame 6 saved 12 | VehicleControl(throttle=0.850000, steer=0.000616, brake=0.000000, hand_brake=False, reverse=False, manual_gear_shift=False, gear=0) Transform(Location(x=-1.980020, y=249.430008, z=0.001437), Rotation(pitch=0.000000, yaw=90.000046, roll=0.000000)) 0.0010412025731056929 30.0 Green False None 13 | Frame 7 saved 14 | VehicleControl(throttle=0.850000, steer=0.000616, brake=0.000000, hand_brake=False, reverse=False, manual_gear_shift=False, gear=0) Transform(Location(x=-1.980019, y=249.430008, z=0.001416), Rotation(pitch=0.000000, yaw=90.000046, roll=0.000000)) 0.0009303855476900935 30.0 Green False None 15 | Frame 8 saved 16 | VehicleControl(throttle=0.850000, steer=0.000440, brake=0.000000, hand_brake=False, reverse=False, manual_gear_shift=False, gear=0) Transform(Location(x=-1.980019, y=249.430008, z=0.001400), Rotation(pitch=0.000000, yaw=90.000046, roll=0.000000)) 0.0006189918494783342 30.0 Green False None 17 | Frame 9 saved 18 | VehicleControl(throttle=0.850000, steer=0.000440, brake=0.000000, hand_brake=False, reverse=False, manual_gear_shift=False, gear=0) Transform(Location(x=-1.980020, y=249.430008, z=0.001411), Rotation(pitch=0.000000, yaw=90.000046, roll=0.000000)) 0.0006481456803157926 30.0 Green False None 19 | Frame 10 saved 20 | VehicleControl(throttle=0.850000, steer=0.000440, brake=0.000000, hand_brake=False, reverse=False, manual_gear_shift=False, gear=0) Transform(Location(x=-1.980020, y=249.430008, z=0.001424), Rotation(pitch=0.000000, yaw=90.000046, roll=0.000000)) 0.0007696437533013523 30.0 Green False None -------------------------------------------------------------------------------- /Carla/retrive_data/agents/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TroddenSpade/Flexible-Conditional-Imitation-Learning/7ce40ffeac69f3ed09afa3523ef32adb91ad5355/Carla/retrive_data/agents/__init__.py -------------------------------------------------------------------------------- /Carla/retrive_data/agents/navigation/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TroddenSpade/Flexible-Conditional-Imitation-Learning/7ce40ffeac69f3ed09afa3523ef32adb91ad5355/Carla/retrive_data/agents/navigation/__init__.py -------------------------------------------------------------------------------- /Carla/retrive_data/agents/navigation/basic_agent.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) # Copyright (c) 2018-2020 CVC. 2 | # 3 | # This work is licensed under the terms of the MIT license. 4 | # For a copy, see . 5 | 6 | """ 7 | This module implements an agent that roams around a track following random 8 | waypoints and avoiding other vehicles. The agent also responds to traffic lights. 9 | It can also make use of the global route planner to follow a specifed route 10 | """ 11 | 12 | import carla 13 | from enum import Enum 14 | from shapely.geometry import Polygon 15 | 16 | from agents.navigation.local_planner import LocalPlanner 17 | from agents.navigation.global_route_planner import GlobalRoutePlanner 18 | from agents.tools.misc import get_speed, is_within_distance, get_trafficlight_trigger_location, compute_distance 19 | 20 | 21 | class BasicAgent(object): 22 | """ 23 | BasicAgent implements an agent that navigates the scene. 24 | This agent respects traffic lights and other vehicles, but ignores stop signs. 25 | It has several functions available to specify the route that the agent must follow, 26 | as well as to change its parameters in case a different driving mode is desired. 27 | """ 28 | 29 | def __init__(self, vehicle, target_speed=20, opt_dict={}): 30 | """ 31 | Initialization the agent paramters, the local and the global planner. 32 | 33 | :param vehicle: actor to apply to agent logic onto 34 | :param target_speed: speed (in Km/h) at which the vehicle will move 35 | :param opt_dict: dictionary in case some of its parameters want to be changed. 36 | This also applies to parameters related to the LocalPlanner. 37 | """ 38 | self._vehicle = vehicle 39 | self._world = self._vehicle.get_world() 40 | self._map = self._world.get_map() 41 | self._last_traffic_light = None 42 | 43 | # Base parameters 44 | self._ignore_traffic_lights = False 45 | self._ignore_stop_signs = False 46 | self._ignore_vehicles = False 47 | self._target_speed = target_speed 48 | self._sampling_resolution = 2.0 49 | self._base_tlight_threshold = 5.0 # meters 50 | self._base_vehicle_threshold = 5.0 # meters 51 | self._max_brake = 0.5 52 | 53 | # Change parameters according to the dictionary 54 | opt_dict['target_speed'] = target_speed 55 | if 'ignore_traffic_lights' in opt_dict: 56 | self._ignore_traffic_lights = opt_dict['ignore_traffic_lights'] 57 | if 'ignore_stop_signs' in opt_dict: 58 | self._ignore_stop_signs = opt_dict['ignore_stop_signs'] 59 | if 'ignore_vehicles' in opt_dict: 60 | self._ignore_vehicles = opt_dict['ignore_vehicles'] 61 | if 'sampling_resolution' in opt_dict: 62 | self._sampling_resolution = opt_dict['sampling_resolution'] 63 | if 'base_tlight_threshold' in opt_dict: 64 | self._base_tlight_threshold = opt_dict['base_tlight_threshold'] 65 | if 'base_vehicle_threshold' in opt_dict: 66 | self._base_vehicle_threshold = opt_dict['base_vehicle_threshold'] 67 | if 'max_brake' in opt_dict: 68 | self._max_steering = opt_dict['max_brake'] 69 | 70 | # Initialize the planners 71 | self._local_planner = LocalPlanner(self._vehicle, opt_dict=opt_dict) 72 | self._global_planner = GlobalRoutePlanner(self._map, self._sampling_resolution) 73 | 74 | def add_emergency_stop(self, control): 75 | """ 76 | Overwrites the throttle a brake values of a control to perform an emergency stop. 77 | The steering is kept the same to avoid going out of the lane when stopping during turns 78 | 79 | :param speed (carl.VehicleControl): control to be modified 80 | """ 81 | control.throttle = 0.0 82 | control.brake = self._max_brake 83 | control.hand_brake = False 84 | return control 85 | 86 | def set_target_speed(self, speed): 87 | """ 88 | Changes the target speed of the agent 89 | :param speed (float): target speed in Km/h 90 | """ 91 | self._local_planner.set_speed(speed) 92 | 93 | def follow_speed_limits(self, value=True): 94 | """ 95 | If active, the agent will dynamically change the target speed according to the speed limits 96 | 97 | :param value (bool): whether or not to activate this behavior 98 | """ 99 | self._local_planner.follow_speed_limits(value) 100 | 101 | def get_local_planner(self): 102 | """Get method for protected member local planner""" 103 | return self._local_planner 104 | 105 | def get_global_planner(self): 106 | """Get method for protected member local planner""" 107 | return self._global_planner 108 | 109 | def set_destination(self, end_location, start_location=None): 110 | """ 111 | This method creates a list of waypoints between a starting and ending location, 112 | based on the route returned by the global router, and adds it to the local planner. 113 | If no starting location is passed, the vehicle local planner's target location is chosen, 114 | which corresponds (by default), to a location about 5 meters in front of the vehicle. 115 | 116 | :param end_location (carla.Location): final location of the route 117 | :param start_location (carla.Location): starting location of the route 118 | """ 119 | if not start_location: 120 | start_location = self._local_planner.target_waypoint.transform.location 121 | clean_queue = True 122 | else: 123 | start_location = self._vehicle.get_location() 124 | clean_queue = False 125 | 126 | start_waypoint = self._map.get_waypoint(start_location) 127 | end_waypoint = self._map.get_waypoint(end_location) 128 | 129 | route_trace = self.trace_route(start_waypoint, end_waypoint) 130 | self._local_planner.set_global_plan(route_trace, clean_queue=clean_queue) 131 | 132 | def set_global_plan(self, plan, stop_waypoint_creation=True, clean_queue=True): 133 | """ 134 | Adds a specific plan to the agent. 135 | 136 | :param plan: list of [carla.Waypoint, RoadOption] representing the route to be followed 137 | :param stop_waypoint_creation: stops the automatic random creation of waypoints 138 | :param clean_queue: resets the current agent's plan 139 | """ 140 | self._local_planner.set_global_plan( 141 | plan, 142 | stop_waypoint_creation=stop_waypoint_creation, 143 | clean_queue=clean_queue 144 | ) 145 | 146 | def trace_route(self, start_waypoint, end_waypoint): 147 | """ 148 | Calculates the shortest route between a starting and ending waypoint. 149 | 150 | :param start_waypoint (carla.Waypoint): initial waypoint 151 | :param end_waypoint (carla.Waypoint): final waypoint 152 | """ 153 | start_location = start_waypoint.transform.location 154 | end_location = end_waypoint.transform.location 155 | return self._global_planner.trace_route(start_location, end_location) 156 | 157 | def run_step(self): 158 | """Execute one step of navigation.""" 159 | hazard_detected = False 160 | 161 | # Retrieve all relevant actors 162 | actor_list = self._world.get_actors() 163 | vehicle_list = actor_list.filter("*vehicle*") 164 | lights_list = actor_list.filter("*traffic_light*") 165 | 166 | vehicle_speed = get_speed(self._vehicle) / 3.6 167 | 168 | # Check for possible vehicle obstacles 169 | max_vehicle_distance = self._base_vehicle_threshold + vehicle_speed 170 | affected_by_vehicle, _, _ = self._vehicle_obstacle_detected(vehicle_list, max_vehicle_distance) 171 | if affected_by_vehicle: 172 | hazard_detected = True 173 | 174 | # Check if the vehicle is affected by a red traffic light 175 | max_tlight_distance = self._base_tlight_threshold + vehicle_speed 176 | affected_by_tlight, _ = self._affected_by_traffic_light(lights_list, max_tlight_distance) 177 | if affected_by_tlight: 178 | hazard_detected = True 179 | 180 | control = self._local_planner.run_step() 181 | if hazard_detected: 182 | control = self.add_emergency_stop(control) 183 | 184 | return control 185 | 186 | def done(self): 187 | """Check whether the agent has reached its destination.""" 188 | return self._local_planner.done() 189 | 190 | def ignore_traffic_lights(self, active=True): 191 | """(De)activates the checks for traffic lights""" 192 | self._ignore_traffic_lights = active 193 | 194 | def ignore_stop_signs(self, active=True): 195 | """(De)activates the checks for stop signs""" 196 | self._ignore_stop_signs = active 197 | 198 | def ignore_vehicles(self, active=True): 199 | """(De)activates the checks for stop signs""" 200 | self._ignore_vehicles = active 201 | 202 | def _affected_by_traffic_light(self, lights_list=None, max_distance=None): 203 | """ 204 | Method to check if there is a red light affecting the vehicle. 205 | 206 | :param lights_list (list of carla.TrafficLight): list containing TrafficLight objects. 207 | If None, all traffic lights in the scene are used 208 | :param max_distance (float): max distance for traffic lights to be considered relevant. 209 | If None, the base threshold value is used 210 | """ 211 | if self._ignore_traffic_lights: 212 | return (False, None) 213 | 214 | if not lights_list: 215 | lights_list = self._world.get_actors().filter("*traffic_light*") 216 | 217 | if not max_distance: 218 | max_distance = self._base_tlight_threshold 219 | 220 | if self._last_traffic_light: 221 | if self._last_traffic_light.state != carla.TrafficLightState.Red: 222 | self._last_traffic_light = None 223 | else: 224 | return (True, self._last_traffic_light) 225 | 226 | ego_vehicle_location = self._vehicle.get_location() 227 | ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location) 228 | 229 | for traffic_light in lights_list: 230 | object_location = get_trafficlight_trigger_location(traffic_light) 231 | object_waypoint = self._map.get_waypoint(object_location) 232 | 233 | if object_waypoint.road_id != ego_vehicle_waypoint.road_id: 234 | continue 235 | 236 | ve_dir = ego_vehicle_waypoint.transform.get_forward_vector() 237 | wp_dir = object_waypoint.transform.get_forward_vector() 238 | dot_ve_wp = ve_dir.x * wp_dir.x + ve_dir.y * wp_dir.y + ve_dir.z * wp_dir.z 239 | 240 | if dot_ve_wp < 0: 241 | continue 242 | 243 | if traffic_light.state != carla.TrafficLightState.Red: 244 | continue 245 | 246 | if is_within_distance(object_waypoint.transform, self._vehicle.get_transform(), max_distance, [0, 90]): 247 | self._last_traffic_light = traffic_light 248 | return (True, traffic_light) 249 | 250 | return (False, None) 251 | 252 | def _vehicle_obstacle_detected(self, vehicle_list=None, max_distance=None, up_angle_th=90, low_angle_th=0, lane_offset=0): 253 | """ 254 | Method to check if there is a vehicle in front of the agent blocking its path. 255 | 256 | :param vehicle_list (list of carla.Vehicle): list contatining vehicle objects. 257 | If None, all vehicle in the scene are used 258 | :param max_distance: max freespace to check for obstacles. 259 | If None, the base threshold value is used 260 | """ 261 | if self._ignore_vehicles: 262 | return (False, None, -1) 263 | 264 | if not vehicle_list: 265 | vehicle_list = self._world.get_actors().filter("*vehicle*") 266 | 267 | if not max_distance: 268 | max_distance = self._base_vehicle_threshold 269 | 270 | ego_transform = self._vehicle.get_transform() 271 | ego_wpt = self._map.get_waypoint(self._vehicle.get_location()) 272 | 273 | # Get the right offset 274 | if ego_wpt.lane_id < 0 and lane_offset != 0: 275 | lane_offset *= -1 276 | 277 | # Get the transform of the front of the ego 278 | ego_forward_vector = ego_transform.get_forward_vector() 279 | ego_extent = self._vehicle.bounding_box.extent.x 280 | ego_front_transform = ego_transform 281 | ego_front_transform.location += carla.Location( 282 | x=ego_extent * ego_forward_vector.x, 283 | y=ego_extent * ego_forward_vector.y, 284 | ) 285 | 286 | for target_vehicle in vehicle_list: 287 | target_transform = target_vehicle.get_transform() 288 | target_wpt = self._map.get_waypoint(target_transform.location, lane_type=carla.LaneType.Any) 289 | 290 | # Simplified version for outside junctions 291 | if not ego_wpt.is_junction or not target_wpt.is_junction: 292 | 293 | if target_wpt.road_id != ego_wpt.road_id or target_wpt.lane_id != ego_wpt.lane_id + lane_offset: 294 | next_wpt = self._local_planner.get_incoming_waypoint_and_direction(steps=3)[0] 295 | if not next_wpt: 296 | continue 297 | if target_wpt.road_id != next_wpt.road_id or target_wpt.lane_id != next_wpt.lane_id + lane_offset: 298 | continue 299 | 300 | target_forward_vector = target_transform.get_forward_vector() 301 | target_extent = target_vehicle.bounding_box.extent.x 302 | target_rear_transform = target_transform 303 | target_rear_transform.location -= carla.Location( 304 | x=target_extent * target_forward_vector.x, 305 | y=target_extent * target_forward_vector.y, 306 | ) 307 | 308 | if is_within_distance(target_rear_transform, ego_front_transform, max_distance, [low_angle_th, up_angle_th]): 309 | return (True, target_vehicle, compute_distance(target_transform.location, ego_transform.location)) 310 | 311 | # Waypoints aren't reliable, check the proximity of the vehicle to the route 312 | else: 313 | route_bb = [] 314 | ego_location = ego_transform.location 315 | extent_y = self._vehicle.bounding_box.extent.y 316 | r_vec = ego_transform.get_right_vector() 317 | p1 = ego_location + carla.Location(extent_y * r_vec.x, extent_y * r_vec.y) 318 | p2 = ego_location + carla.Location(-extent_y * r_vec.x, -extent_y * r_vec.y) 319 | route_bb.append([p1.x, p1.y, p1.z]) 320 | route_bb.append([p2.x, p2.y, p2.z]) 321 | 322 | for wp, _ in self._local_planner.get_plan(): 323 | if ego_location.distance(wp.transform.location) > max_distance: 324 | break 325 | 326 | r_vec = wp.transform.get_right_vector() 327 | p1 = wp.transform.location + carla.Location(extent_y * r_vec.x, extent_y * r_vec.y) 328 | p2 = wp.transform.location + carla.Location(-extent_y * r_vec.x, -extent_y * r_vec.y) 329 | route_bb.append([p1.x, p1.y, p1.z]) 330 | route_bb.append([p2.x, p2.y, p2.z]) 331 | 332 | if len(route_bb) < 3: 333 | # 2 points don't create a polygon, nothing to check 334 | return (False, None, -1) 335 | ego_polygon = Polygon(route_bb) 336 | 337 | # Compare the two polygons 338 | for target_vehicle in vehicle_list: 339 | target_extent = target_vehicle.bounding_box.extent.x 340 | if target_vehicle.id == self._vehicle.id: 341 | continue 342 | if ego_location.distance(target_vehicle.get_location()) > max_distance: 343 | continue 344 | 345 | target_bb = target_vehicle.bounding_box 346 | target_vertices = target_bb.get_world_vertices(target_vehicle.get_transform()) 347 | target_list = [[v.x, v.y, v.z] for v in target_vertices] 348 | target_polygon = Polygon(target_list) 349 | 350 | if ego_polygon.intersects(target_polygon): 351 | return (True, target_vehicle, compute_distance(target_vehicle.get_location(), ego_location)) 352 | 353 | return (False, None, -1) 354 | 355 | return (False, None, -1) 356 | -------------------------------------------------------------------------------- /Carla/retrive_data/agents/navigation/behavior_agent.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) # Copyright (c) 2018-2020 CVC. 2 | # 3 | # This work is licensed under the terms of the MIT license. 4 | # For a copy, see . 5 | 6 | 7 | """ This module implements an agent that roams around a track following random 8 | waypoints and avoiding other vehicles. The agent also responds to traffic lights, 9 | traffic signs, and has different possible configurations. """ 10 | 11 | import random 12 | import numpy as np 13 | import carla 14 | from agents.navigation.basic_agent import BasicAgent 15 | from agents.navigation.local_planner import RoadOption 16 | from agents.navigation.behavior_types import Cautious, Aggressive, Normal 17 | 18 | from agents.tools.misc import get_speed, positive, is_within_distance, compute_distance 19 | 20 | class BehaviorAgent(BasicAgent): 21 | """ 22 | BehaviorAgent implements an agent that navigates scenes to reach a given 23 | target destination, by computing the shortest possible path to it. 24 | This agent can correctly follow traffic signs, speed limitations, 25 | traffic lights, while also taking into account nearby vehicles. Lane changing 26 | decisions can be taken by analyzing the surrounding environment such as tailgating avoidance. 27 | Adding to these are possible behaviors, the agent can also keep safety distance 28 | from a car in front of it by tracking the instantaneous time to collision 29 | and keeping it in a certain range. Finally, different sets of behaviors 30 | are encoded in the agent, from cautious to a more aggressive ones. 31 | """ 32 | 33 | def __init__(self, vehicle, behavior='normal'): 34 | """ 35 | Constructor method. 36 | 37 | :param vehicle: actor to apply to local planner logic onto 38 | :param ignore_traffic_light: boolean to ignore any traffic light 39 | :param behavior: type of agent to apply 40 | """ 41 | 42 | super(BehaviorAgent, self).__init__(vehicle) 43 | self._look_ahead_steps = 0 44 | 45 | # Vehicle information 46 | self._speed = 0 47 | self._speed_limit = 0 48 | self._direction = None 49 | self._incoming_direction = None 50 | self._incoming_waypoint = None 51 | self._min_speed = 5 52 | self._behavior = None 53 | self._sampling_resolution = 4.5 54 | 55 | # Parameters for agent behavior 56 | if behavior == 'cautious': 57 | self._behavior = Cautious() 58 | 59 | elif behavior == 'normal': 60 | self._behavior = Normal() 61 | 62 | elif behavior == 'aggressive': 63 | self._behavior = Aggressive() 64 | 65 | def _update_information(self): 66 | """ 67 | This method updates the information regarding the ego 68 | vehicle based on the surrounding world. 69 | """ 70 | self._speed = get_speed(self._vehicle) 71 | self._speed_limit = self._vehicle.get_speed_limit() 72 | self._local_planner.set_speed(self._speed_limit) 73 | self._direction = self._local_planner.target_road_option 74 | if self._direction is None: 75 | self._direction = RoadOption.LANEFOLLOW 76 | 77 | self._look_ahead_steps = int((self._speed_limit) / 10) 78 | 79 | self._incoming_waypoint, self._incoming_direction = self._local_planner.get_incoming_waypoint_and_direction( 80 | steps=self._look_ahead_steps) 81 | if self._incoming_direction is None: 82 | self._incoming_direction = RoadOption.LANEFOLLOW 83 | 84 | def traffic_light_manager(self): 85 | """ 86 | This method is in charge of behaviors for red lights. 87 | """ 88 | actor_list = self._world.get_actors() 89 | lights_list = actor_list.filter("*traffic_light*") 90 | affected, _ = self._affected_by_traffic_light(lights_list) 91 | 92 | return affected 93 | 94 | def _tailgating(self, waypoint, vehicle_list): 95 | """ 96 | This method is in charge of tailgating behaviors. 97 | 98 | :param location: current location of the agent 99 | :param waypoint: current waypoint of the agent 100 | :param vehicle_list: list of all the nearby vehicles 101 | """ 102 | 103 | left_turn = waypoint.left_lane_marking.lane_change 104 | right_turn = waypoint.right_lane_marking.lane_change 105 | 106 | left_wpt = waypoint.get_left_lane() 107 | right_wpt = waypoint.get_right_lane() 108 | 109 | behind_vehicle_state, behind_vehicle, _ = self._vehicle_obstacle_detected(vehicle_list, max( 110 | self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=180, low_angle_th=160) 111 | if behind_vehicle_state and self._speed < get_speed(behind_vehicle): 112 | if (right_turn == carla.LaneChange.Right or right_turn == 113 | carla.LaneChange.Both) and waypoint.lane_id * right_wpt.lane_id > 0 and right_wpt.lane_type == carla.LaneType.Driving: 114 | new_vehicle_state, _, _ = self._vehicle_obstacle_detected(vehicle_list, max( 115 | self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=180, lane_offset=1) 116 | if not new_vehicle_state: 117 | print("Tailgating, moving to the right!") 118 | end_waypoint = self._local_planner.target_waypoint 119 | self._behavior.tailgate_counter = 200 120 | self.set_destination(end_waypoint.transform.location, 121 | right_wpt.transform.location) 122 | elif left_turn == carla.LaneChange.Left and waypoint.lane_id * left_wpt.lane_id > 0 and left_wpt.lane_type == carla.LaneType.Driving: 123 | new_vehicle_state, _, _ = self._vehicle_obstacle_detected(vehicle_list, max( 124 | self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=180, lane_offset=-1) 125 | if not new_vehicle_state: 126 | print("Tailgating, moving to the left!") 127 | end_waypoint = self._local_planner.target_waypoint 128 | self._behavior.tailgate_counter = 200 129 | self.set_destination(end_waypoint.transform.location, 130 | left_wpt.transform.location) 131 | 132 | def collision_and_car_avoid_manager(self, waypoint): 133 | """ 134 | This module is in charge of warning in case of a collision 135 | and managing possible tailgating chances. 136 | 137 | :param location: current location of the agent 138 | :param waypoint: current waypoint of the agent 139 | :return vehicle_state: True if there is a vehicle nearby, False if not 140 | :return vehicle: nearby vehicle 141 | :return distance: distance to nearby vehicle 142 | """ 143 | 144 | vehicle_list = self._world.get_actors().filter("*vehicle*") 145 | def dist(v): return v.get_location().distance(waypoint.transform.location) 146 | vehicle_list = [v for v in vehicle_list if dist(v) < 45 and v.id != self._vehicle.id] 147 | 148 | if self._direction == RoadOption.CHANGELANELEFT: 149 | vehicle_state, vehicle, distance = self._vehicle_obstacle_detected( 150 | vehicle_list, max( 151 | self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=180, lane_offset=-1) 152 | elif self._direction == RoadOption.CHANGELANERIGHT: 153 | vehicle_state, vehicle, distance = self._vehicle_obstacle_detected( 154 | vehicle_list, max( 155 | self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=180, lane_offset=1) 156 | else: 157 | vehicle_state, vehicle, distance = self._vehicle_obstacle_detected( 158 | vehicle_list, max( 159 | self._behavior.min_proximity_threshold, self._speed_limit / 3), up_angle_th=30) 160 | 161 | # Check for tailgating 162 | if not vehicle_state and self._direction == RoadOption.LANEFOLLOW \ 163 | and not waypoint.is_junction and self._speed > 10 \ 164 | and self._behavior.tailgate_counter == 0: 165 | self._tailgating(waypoint, vehicle_list) 166 | 167 | return vehicle_state, vehicle, distance 168 | 169 | def pedestrian_avoid_manager(self, waypoint): 170 | """ 171 | This module is in charge of warning in case of a collision 172 | with any pedestrian. 173 | 174 | :param location: current location of the agent 175 | :param waypoint: current waypoint of the agent 176 | :return vehicle_state: True if there is a walker nearby, False if not 177 | :return vehicle: nearby walker 178 | :return distance: distance to nearby walker 179 | """ 180 | 181 | walker_list = self._world.get_actors().filter("*walker.pedestrian*") 182 | def dist(w): return w.get_location().distance(waypoint.transform.location) 183 | walker_list = [w for w in walker_list if dist(w) < 10] 184 | 185 | if self._direction == RoadOption.CHANGELANELEFT: 186 | walker_state, walker, distance = self._vehicle_obstacle_detected(walker_list, max( 187 | self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=90, lane_offset=-1) 188 | elif self._direction == RoadOption.CHANGELANERIGHT: 189 | walker_state, walker, distance = self._vehicle_obstacle_detected(walker_list, max( 190 | self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=90, lane_offset=1) 191 | else: 192 | walker_state, walker, distance = self._vehicle_obstacle_detected(walker_list, max( 193 | self._behavior.min_proximity_threshold, self._speed_limit / 3), up_angle_th=60) 194 | 195 | return walker_state, walker, distance 196 | 197 | def car_following_manager(self, vehicle, distance, debug=False): 198 | """ 199 | Module in charge of car-following behaviors when there's 200 | someone in front of us. 201 | 202 | :param vehicle: car to follow 203 | :param distance: distance from vehicle 204 | :param debug: boolean for debugging 205 | :return control: carla.VehicleControl 206 | """ 207 | 208 | vehicle_speed = get_speed(vehicle) 209 | delta_v = max(1, (self._speed - vehicle_speed) / 3.6) 210 | ttc = distance / delta_v if delta_v != 0 else distance / np.nextafter(0., 1.) 211 | 212 | # Under safety time distance, slow down. 213 | if self._behavior.safety_time > ttc > 0.0: 214 | target_speed = min([ 215 | positive(vehicle_speed - self._behavior.speed_decrease), 216 | self._behavior.max_speed, 217 | self._speed_limit - self._behavior.speed_lim_dist]) 218 | self._local_planner.set_speed(target_speed) 219 | control = self._local_planner.run_step(debug=debug) 220 | 221 | # Actual safety distance area, try to follow the speed of the vehicle in front. 222 | elif 2 * self._behavior.safety_time > ttc >= self._behavior.safety_time: 223 | target_speed = min([ 224 | max(self._min_speed, vehicle_speed), 225 | self._behavior.max_speed, 226 | self._speed_limit - self._behavior.speed_lim_dist]) 227 | self._local_planner.set_speed(target_speed) 228 | control = self._local_planner.run_step(debug=debug) 229 | 230 | # Normal behavior. 231 | else: 232 | target_speed = min([ 233 | self._behavior.max_speed, 234 | self._speed_limit - self._behavior.speed_lim_dist]) 235 | self._local_planner.set_speed(target_speed) 236 | control = self._local_planner.run_step(debug=debug) 237 | 238 | return control 239 | 240 | def run_step(self, debug=False): 241 | """ 242 | Execute one step of navigation. 243 | 244 | :param debug: boolean for debugging 245 | :return control: carla.VehicleControl 246 | """ 247 | self._update_information() 248 | 249 | control = None 250 | if self._behavior.tailgate_counter > 0: 251 | self._behavior.tailgate_counter -= 1 252 | 253 | ego_vehicle_loc = self._vehicle.get_location() 254 | ego_vehicle_wp = self._map.get_waypoint(ego_vehicle_loc) 255 | 256 | # 1: Red lights and stops behavior 257 | if self.traffic_light_manager(): 258 | return self.emergency_stop() 259 | 260 | # 2.1: Pedestrian avoidance behaviors 261 | walker_state, walker, w_distance = self.pedestrian_avoid_manager(ego_vehicle_wp) 262 | 263 | if walker_state: 264 | # Distance is computed from the center of the two cars, 265 | # we use bounding boxes to calculate the actual distance 266 | distance = w_distance - max( 267 | walker.bounding_box.extent.y, walker.bounding_box.extent.x) - max( 268 | self._vehicle.bounding_box.extent.y, self._vehicle.bounding_box.extent.x) 269 | 270 | # Emergency brake if the car is very close. 271 | if distance < self._behavior.braking_distance: 272 | return self.emergency_stop() 273 | 274 | # 2.2: Car following behaviors 275 | vehicle_state, vehicle, distance = self.collision_and_car_avoid_manager(ego_vehicle_wp) 276 | 277 | if vehicle_state: 278 | # Distance is computed from the center of the two cars, 279 | # we use bounding boxes to calculate the actual distance 280 | distance = distance - max( 281 | vehicle.bounding_box.extent.y, vehicle.bounding_box.extent.x) - max( 282 | self._vehicle.bounding_box.extent.y, self._vehicle.bounding_box.extent.x) 283 | 284 | # Emergency brake if the car is very close. 285 | if distance < self._behavior.braking_distance: 286 | return self.emergency_stop() 287 | else: 288 | control = self.car_following_manager(vehicle, distance) 289 | 290 | # 3: Intersection behavior 291 | elif self._incoming_waypoint.is_junction and (self._incoming_direction in [RoadOption.LEFT, RoadOption.RIGHT]): 292 | target_speed = min([ 293 | self._behavior.max_speed, 294 | self._speed_limit - 5]) 295 | self._local_planner.set_speed(target_speed) 296 | control = self._local_planner.run_step(debug=debug) 297 | 298 | # 4: Normal behavior 299 | else: 300 | target_speed = min([ 301 | self._behavior.max_speed, 302 | self._speed_limit - self._behavior.speed_lim_dist]) 303 | self._local_planner.set_speed(target_speed) 304 | control = self._local_planner.run_step(debug=debug) 305 | 306 | return control 307 | 308 | def emergency_stop(self): 309 | """ 310 | Overwrites the throttle a brake values of a control to perform an emergency stop. 311 | The steering is kept the same to avoid going out of the lane when stopping during turns 312 | 313 | :param speed (carl.VehicleControl): control to be modified 314 | """ 315 | control = carla.VehicleControl() 316 | control.throttle = 0.0 317 | control.brake = self._max_brake 318 | control.hand_brake = False 319 | return control -------------------------------------------------------------------------------- /Carla/retrive_data/agents/navigation/behavior_types.py: -------------------------------------------------------------------------------- 1 | # This work is licensed under the terms of the MIT license. 2 | # For a copy, see . 3 | 4 | """ This module contains the different parameters sets for each behavior. """ 5 | 6 | 7 | class Cautious(object): 8 | """Class for Cautious agent.""" 9 | max_speed = 40 10 | speed_lim_dist = 6 11 | speed_decrease = 12 12 | safety_time = 3 13 | min_proximity_threshold = 12 14 | braking_distance = 6 15 | tailgate_counter = 0 16 | 17 | 18 | class Normal(object): 19 | """Class for Normal agent.""" 20 | max_speed = 50 21 | speed_lim_dist = 3 22 | speed_decrease = 10 23 | safety_time = 3 24 | min_proximity_threshold = 10 25 | braking_distance = 5 26 | tailgate_counter = 0 27 | 28 | 29 | class Aggressive(object): 30 | """Class for Aggressive agent.""" 31 | max_speed = 70 32 | speed_lim_dist = 1 33 | speed_decrease = 8 34 | safety_time = 3 35 | min_proximity_threshold = 8 36 | braking_distance = 4 37 | tailgate_counter = -1 38 | -------------------------------------------------------------------------------- /Carla/retrive_data/agents/navigation/controller.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) # Copyright (c) 2018-2020 CVC. 2 | # 3 | # This work is licensed under the terms of the MIT license. 4 | # For a copy, see . 5 | 6 | """ This module contains PID controllers to perform lateral and longitudinal control. """ 7 | 8 | from collections import deque 9 | import math 10 | import numpy as np 11 | import carla 12 | from agents.tools.misc import get_speed 13 | 14 | 15 | class VehiclePIDController(): 16 | """ 17 | VehiclePIDController is the combination of two PID controllers 18 | (lateral and longitudinal) to perform the 19 | low level control a vehicle from client side 20 | """ 21 | 22 | 23 | def __init__(self, vehicle, args_lateral, args_longitudinal, offset=0, max_throttle=0.75, max_brake=0.3, 24 | max_steering=0.8): 25 | """ 26 | Constructor method. 27 | 28 | :param vehicle: actor to apply to local planner logic onto 29 | :param args_lateral: dictionary of arguments to set the lateral PID controller 30 | using the following semantics: 31 | K_P -- Proportional term 32 | K_D -- Differential term 33 | K_I -- Integral term 34 | :param args_longitudinal: dictionary of arguments to set the longitudinal 35 | PID controller using the following semantics: 36 | K_P -- Proportional term 37 | K_D -- Differential term 38 | K_I -- Integral term 39 | :param offset: If different than zero, the vehicle will drive displaced from the center line. 40 | Positive values imply a right offset while negative ones mean a left one. Numbers high enough 41 | to cause the vehicle to drive through other lanes might break the controller. 42 | """ 43 | 44 | self.max_brake = max_brake 45 | self.max_throt = max_throttle 46 | self.max_steer = max_steering 47 | 48 | self._vehicle = vehicle 49 | self._world = self._vehicle.get_world() 50 | self.past_steering = self._vehicle.get_control().steer 51 | self._lon_controller = PIDLongitudinalController(self._vehicle, **args_longitudinal) 52 | self._lat_controller = PIDLateralController(self._vehicle, offset, **args_lateral) 53 | 54 | def run_step(self, target_speed, waypoint): 55 | """ 56 | Execute one step of control invoking both lateral and longitudinal 57 | PID controllers to reach a target waypoint 58 | at a given target_speed. 59 | 60 | :param target_speed: desired vehicle speed 61 | :param waypoint: target location encoded as a waypoint 62 | :return: distance (in meters) to the waypoint 63 | """ 64 | 65 | acceleration = self._lon_controller.run_step(target_speed) 66 | current_steering = self._lat_controller.run_step(waypoint) 67 | control = carla.VehicleControl() 68 | if acceleration >= 0.0: 69 | control.throttle = min(acceleration, self.max_throt) 70 | control.brake = 0.0 71 | else: 72 | control.throttle = 0.0 73 | control.brake = min(abs(acceleration), self.max_brake) 74 | 75 | # Steering regulation: changes cannot happen abruptly, can't steer too much. 76 | 77 | if current_steering > self.past_steering + 0.1: 78 | current_steering = self.past_steering + 0.1 79 | elif current_steering < self.past_steering - 0.1: 80 | current_steering = self.past_steering - 0.1 81 | 82 | if current_steering >= 0: 83 | steering = min(self.max_steer, current_steering) 84 | else: 85 | steering = max(-self.max_steer, current_steering) 86 | 87 | control.steer = steering 88 | control.hand_brake = False 89 | control.manual_gear_shift = False 90 | self.past_steering = steering 91 | 92 | return control 93 | 94 | 95 | def change_longitudinal_PID(self, args_longitudinal): 96 | """Changes the parameters of the PIDLongitudinalController""" 97 | self._lon_controller.change_parameters(**args_longitudinal) 98 | 99 | def change_lateral_PID(self, args_lateral): 100 | """Changes the parameters of the PIDLongitudinalController""" 101 | self._lon_controller.change_parameters(**args_lateral) 102 | 103 | 104 | class PIDLongitudinalController(): 105 | """ 106 | PIDLongitudinalController implements longitudinal control using a PID. 107 | """ 108 | 109 | def __init__(self, vehicle, K_P=1.0, K_I=0.0, K_D=0.0, dt=0.03): 110 | """ 111 | Constructor method. 112 | 113 | :param vehicle: actor to apply to local planner logic onto 114 | :param K_P: Proportional term 115 | :param K_D: Differential term 116 | :param K_I: Integral term 117 | :param dt: time differential in seconds 118 | """ 119 | self._vehicle = vehicle 120 | self._k_p = K_P 121 | self._k_i = K_I 122 | self._k_d = K_D 123 | self._dt = dt 124 | self._error_buffer = deque(maxlen=10) 125 | 126 | def run_step(self, target_speed, debug=False): 127 | """ 128 | Execute one step of longitudinal control to reach a given target speed. 129 | 130 | :param target_speed: target speed in Km/h 131 | :param debug: boolean for debugging 132 | :return: throttle control 133 | """ 134 | current_speed = get_speed(self._vehicle) 135 | 136 | if debug: 137 | print('Current speed = {}'.format(current_speed)) 138 | 139 | return self._pid_control(target_speed, current_speed) 140 | 141 | def _pid_control(self, target_speed, current_speed): 142 | """ 143 | Estimate the throttle/brake of the vehicle based on the PID equations 144 | 145 | :param target_speed: target speed in Km/h 146 | :param current_speed: current speed of the vehicle in Km/h 147 | :return: throttle/brake control 148 | """ 149 | 150 | error = target_speed - current_speed 151 | self._error_buffer.append(error) 152 | 153 | if len(self._error_buffer) >= 2: 154 | _de = (self._error_buffer[-1] - self._error_buffer[-2]) / self._dt 155 | _ie = sum(self._error_buffer) * self._dt 156 | else: 157 | _de = 0.0 158 | _ie = 0.0 159 | 160 | return np.clip((self._k_p * error) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0) 161 | 162 | def change_parameters(self, K_P, K_I, K_D, dt): 163 | """Changes the PID parameters""" 164 | self._k_p = K_P 165 | self._k_i = K_I 166 | self._k_d = K_D 167 | self._dt = dt 168 | 169 | 170 | class PIDLateralController(): 171 | """ 172 | PIDLateralController implements lateral control using a PID. 173 | """ 174 | 175 | def __init__(self, vehicle, offset=0, K_P=1.0, K_I=0.0, K_D=0.0, dt=0.03): 176 | """ 177 | Constructor method. 178 | 179 | :param vehicle: actor to apply to local planner logic onto 180 | :param offset: distance to the center line. If might cause issues if the value 181 | is large enough to make the vehicle invade other lanes. 182 | :param K_P: Proportional term 183 | :param K_D: Differential term 184 | :param K_I: Integral term 185 | :param dt: time differential in seconds 186 | """ 187 | self._vehicle = vehicle 188 | self._k_p = K_P 189 | self._k_i = K_I 190 | self._k_d = K_D 191 | self._dt = dt 192 | self._offset = offset 193 | self._e_buffer = deque(maxlen=10) 194 | 195 | def run_step(self, waypoint): 196 | """ 197 | Execute one step of lateral control to steer 198 | the vehicle towards a certain waypoin. 199 | 200 | :param waypoint: target waypoint 201 | :return: steering control in the range [-1, 1] where: 202 | -1 maximum steering to left 203 | +1 maximum steering to right 204 | """ 205 | return self._pid_control(waypoint, self._vehicle.get_transform()) 206 | 207 | def _pid_control(self, waypoint, vehicle_transform): 208 | """ 209 | Estimate the steering angle of the vehicle based on the PID equations 210 | 211 | :param waypoint: target waypoint 212 | :param vehicle_transform: current transform of the vehicle 213 | :return: steering control in the range [-1, 1] 214 | """ 215 | # Get the ego's location and forward vector 216 | ego_loc = vehicle_transform.location 217 | v_vec = vehicle_transform.get_forward_vector() 218 | v_vec = np.array([v_vec.x, v_vec.y, 0.0]) 219 | 220 | # Get the vector vehicle-target_wp 221 | if self._offset != 0: 222 | # Displace the wp to the side 223 | w_tran = waypoint.transform 224 | r_vec = w_tran.get_right_vector() 225 | w_loc = w_tran.location + carla.Location(x=self._offset*r_vec.x, 226 | y=self._offset*r_vec.y) 227 | else: 228 | w_loc = waypoint.transform.location 229 | 230 | w_vec = np.array([w_loc.x - ego_loc.x, 231 | w_loc.y - ego_loc.y, 232 | 0.0]) 233 | 234 | wv_linalg = np.linalg.norm(w_vec) * np.linalg.norm(v_vec) 235 | if wv_linalg == 0: 236 | _dot = 1 237 | else: 238 | _dot = math.acos(np.clip(np.dot(w_vec, v_vec) / (wv_linalg), -1.0, 1.0)) 239 | _cross = np.cross(v_vec, w_vec) 240 | if _cross[2] < 0: 241 | _dot *= -1.0 242 | 243 | self._e_buffer.append(_dot) 244 | if len(self._e_buffer) >= 2: 245 | _de = (self._e_buffer[-1] - self._e_buffer[-2]) / self._dt 246 | _ie = sum(self._e_buffer) * self._dt 247 | else: 248 | _de = 0.0 249 | _ie = 0.0 250 | 251 | return np.clip((self._k_p * _dot) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0) 252 | 253 | def change_parameters(self, K_P, K_I, K_D, dt): 254 | """Changes the PID parameters""" 255 | self._k_p = K_P 256 | self._k_i = K_I 257 | self._k_d = K_D 258 | self._dt = dt -------------------------------------------------------------------------------- /Carla/retrive_data/agents/navigation/global_route_planner.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) # Copyright (c) 2018-2020 CVC. 2 | # 3 | # This work is licensed under the terms of the MIT license. 4 | # For a copy, see . 5 | 6 | 7 | """ 8 | This module provides GlobalRoutePlanner implementation. 9 | """ 10 | 11 | import math 12 | import numpy as np 13 | import networkx as nx 14 | 15 | import carla 16 | from agents.navigation.local_planner import RoadOption 17 | from agents.tools.misc import vector 18 | 19 | class GlobalRoutePlanner(object): 20 | """ 21 | This class provides a very high level route plan. 22 | """ 23 | 24 | def __init__(self, wmap, sampling_resolution): 25 | self._sampling_resolution = sampling_resolution 26 | self._wmap = wmap 27 | self._topology = None 28 | self._graph = None 29 | self._id_map = None 30 | self._road_id_to_edge = None 31 | 32 | self._intersection_end_node = -1 33 | self._previous_decision = RoadOption.VOID 34 | 35 | # Build the graph 36 | self._build_topology() 37 | self._build_graph() 38 | self._find_loose_ends() 39 | self._lane_change_link() 40 | 41 | def trace_route(self, origin, destination): 42 | """ 43 | This method returns list of (carla.Waypoint, RoadOption) 44 | from origin to destination 45 | """ 46 | route_trace = [] 47 | route = self._path_search(origin, destination) 48 | current_waypoint = self._wmap.get_waypoint(origin) 49 | destination_waypoint = self._wmap.get_waypoint(destination) 50 | 51 | for i in range(len(route) - 1): 52 | road_option = self._turn_decision(i, route) 53 | edge = self._graph.edges[route[i], route[i+1]] 54 | path = [] 55 | 56 | if edge['type'] != RoadOption.LANEFOLLOW and edge['type'] != RoadOption.VOID: 57 | route_trace.append((current_waypoint, road_option)) 58 | exit_wp = edge['exit_waypoint'] 59 | n1, n2 = self._road_id_to_edge[exit_wp.road_id][exit_wp.section_id][exit_wp.lane_id] 60 | next_edge = self._graph.edges[n1, n2] 61 | if next_edge['path']: 62 | closest_index = self._find_closest_in_list(current_waypoint, next_edge['path']) 63 | closest_index = min(len(next_edge['path'])-1, closest_index+5) 64 | current_waypoint = next_edge['path'][closest_index] 65 | else: 66 | current_waypoint = next_edge['exit_waypoint'] 67 | route_trace.append((current_waypoint, road_option)) 68 | 69 | else: 70 | path = path + [edge['entry_waypoint']] + edge['path'] + [edge['exit_waypoint']] 71 | closest_index = self._find_closest_in_list(current_waypoint, path) 72 | for waypoint in path[closest_index:]: 73 | current_waypoint = waypoint 74 | route_trace.append((current_waypoint, road_option)) 75 | if len(route)-i <= 2 and waypoint.transform.location.distance(destination) < 2*self._sampling_resolution: 76 | break 77 | elif len(route)-i <= 2 and current_waypoint.road_id == destination_waypoint.road_id and current_waypoint.section_id == destination_waypoint.section_id and current_waypoint.lane_id == destination_waypoint.lane_id: 78 | destination_index = self._find_closest_in_list(destination_waypoint, path) 79 | if closest_index > destination_index: 80 | break 81 | 82 | return route_trace 83 | 84 | def _build_topology(self): 85 | """ 86 | This function retrieves topology from the server as a list of 87 | road segments as pairs of waypoint objects, and processes the 88 | topology into a list of dictionary objects with the following attributes 89 | 90 | - entry (carla.Waypoint): waypoint of entry point of road segment 91 | - entryxyz (tuple): (x,y,z) of entry point of road segment 92 | - exit (carla.Waypoint): waypoint of exit point of road segment 93 | - exitxyz (tuple): (x,y,z) of exit point of road segment 94 | - path (list of carla.Waypoint): list of waypoints between entry to exit, separated by the resolution 95 | """ 96 | self._topology = [] 97 | # Retrieving waypoints to construct a detailed topology 98 | for segment in self._wmap.get_topology(): 99 | wp1, wp2 = segment[0], segment[1] 100 | l1, l2 = wp1.transform.location, wp2.transform.location 101 | # Rounding off to avoid floating point imprecision 102 | x1, y1, z1, x2, y2, z2 = np.round([l1.x, l1.y, l1.z, l2.x, l2.y, l2.z], 0) 103 | wp1.transform.location, wp2.transform.location = l1, l2 104 | seg_dict = dict() 105 | seg_dict['entry'], seg_dict['exit'] = wp1, wp2 106 | seg_dict['entryxyz'], seg_dict['exitxyz'] = (x1, y1, z1), (x2, y2, z2) 107 | seg_dict['path'] = [] 108 | endloc = wp2.transform.location 109 | if wp1.transform.location.distance(endloc) > self._sampling_resolution: 110 | w = wp1.next(self._sampling_resolution)[0] 111 | while w.transform.location.distance(endloc) > self._sampling_resolution: 112 | seg_dict['path'].append(w) 113 | w = w.next(self._sampling_resolution)[0] 114 | else: 115 | seg_dict['path'].append(wp1.next(self._sampling_resolution)[0]) 116 | self._topology.append(seg_dict) 117 | 118 | def _build_graph(self): 119 | """ 120 | This function builds a networkx graph representation of topology, creating several class attributes: 121 | - graph (networkx.DiGraph): networkx graph representing the world map, with: 122 | Node properties: 123 | vertex: (x,y,z) position in world map 124 | Edge properties: 125 | entry_vector: unit vector along tangent at entry point 126 | exit_vector: unit vector along tangent at exit point 127 | net_vector: unit vector of the chord from entry to exit 128 | intersection: boolean indicating if the edge belongs to an intersection 129 | - id_map (dictionary): mapping from (x,y,z) to node id 130 | - road_id_to_edge (dictionary): map from road id to edge in the graph 131 | """ 132 | 133 | self._graph = nx.DiGraph() 134 | self._id_map = dict() # Map with structure {(x,y,z): id, ... } 135 | self._road_id_to_edge = dict() # Map with structure {road_id: {lane_id: edge, ... }, ... } 136 | 137 | for segment in self._topology: 138 | entry_xyz, exit_xyz = segment['entryxyz'], segment['exitxyz'] 139 | path = segment['path'] 140 | entry_wp, exit_wp = segment['entry'], segment['exit'] 141 | intersection = entry_wp.is_junction 142 | road_id, section_id, lane_id = entry_wp.road_id, entry_wp.section_id, entry_wp.lane_id 143 | 144 | for vertex in entry_xyz, exit_xyz: 145 | # Adding unique nodes and populating id_map 146 | if vertex not in self._id_map: 147 | new_id = len(self._id_map) 148 | self._id_map[vertex] = new_id 149 | self._graph.add_node(new_id, vertex=vertex) 150 | n1 = self._id_map[entry_xyz] 151 | n2 = self._id_map[exit_xyz] 152 | if road_id not in self._road_id_to_edge: 153 | self._road_id_to_edge[road_id] = dict() 154 | if section_id not in self._road_id_to_edge[road_id]: 155 | self._road_id_to_edge[road_id][section_id] = dict() 156 | self._road_id_to_edge[road_id][section_id][lane_id] = (n1, n2) 157 | 158 | entry_carla_vector = entry_wp.transform.rotation.get_forward_vector() 159 | exit_carla_vector = exit_wp.transform.rotation.get_forward_vector() 160 | 161 | # Adding edge with attributes 162 | self._graph.add_edge( 163 | n1, n2, 164 | length=len(path) + 1, path=path, 165 | entry_waypoint=entry_wp, exit_waypoint=exit_wp, 166 | entry_vector=np.array( 167 | [entry_carla_vector.x, entry_carla_vector.y, entry_carla_vector.z]), 168 | exit_vector=np.array( 169 | [exit_carla_vector.x, exit_carla_vector.y, exit_carla_vector.z]), 170 | net_vector=vector(entry_wp.transform.location, exit_wp.transform.location), 171 | intersection=intersection, type=RoadOption.LANEFOLLOW) 172 | 173 | def _find_loose_ends(self): 174 | """ 175 | This method finds road segments that have an unconnected end, and 176 | adds them to the internal graph representation 177 | """ 178 | count_loose_ends = 0 179 | hop_resolution = self._sampling_resolution 180 | for segment in self._topology: 181 | end_wp = segment['exit'] 182 | exit_xyz = segment['exitxyz'] 183 | road_id, section_id, lane_id = end_wp.road_id, end_wp.section_id, end_wp.lane_id 184 | if road_id in self._road_id_to_edge \ 185 | and section_id in self._road_id_to_edge[road_id] \ 186 | and lane_id in self._road_id_to_edge[road_id][section_id]: 187 | pass 188 | else: 189 | count_loose_ends += 1 190 | if road_id not in self._road_id_to_edge: 191 | self._road_id_to_edge[road_id] = dict() 192 | if section_id not in self._road_id_to_edge[road_id]: 193 | self._road_id_to_edge[road_id][section_id] = dict() 194 | n1 = self._id_map[exit_xyz] 195 | n2 = -1*count_loose_ends 196 | self._road_id_to_edge[road_id][section_id][lane_id] = (n1, n2) 197 | next_wp = end_wp.next(hop_resolution) 198 | path = [] 199 | while next_wp is not None and next_wp \ 200 | and next_wp[0].road_id == road_id \ 201 | and next_wp[0].section_id == section_id \ 202 | and next_wp[0].lane_id == lane_id: 203 | path.append(next_wp[0]) 204 | next_wp = next_wp[0].next(hop_resolution) 205 | if path: 206 | n2_xyz = (path[-1].transform.location.x, 207 | path[-1].transform.location.y, 208 | path[-1].transform.location.z) 209 | self._graph.add_node(n2, vertex=n2_xyz) 210 | self._graph.add_edge( 211 | n1, n2, 212 | length=len(path) + 1, path=path, 213 | entry_waypoint=end_wp, exit_waypoint=path[-1], 214 | entry_vector=None, exit_vector=None, net_vector=None, 215 | intersection=end_wp.is_junction, type=RoadOption.LANEFOLLOW) 216 | 217 | def _lane_change_link(self): 218 | """ 219 | This method places zero cost links in the topology graph 220 | representing availability of lane changes. 221 | """ 222 | 223 | for segment in self._topology: 224 | left_found, right_found = False, False 225 | 226 | for waypoint in segment['path']: 227 | if not segment['entry'].is_junction: 228 | next_waypoint, next_road_option, next_segment = None, None, None 229 | 230 | if waypoint.right_lane_marking and waypoint.right_lane_marking.lane_change & carla.LaneChange.Right and not right_found: 231 | next_waypoint = waypoint.get_right_lane() 232 | if next_waypoint is not None \ 233 | and next_waypoint.lane_type == carla.LaneType.Driving \ 234 | and waypoint.road_id == next_waypoint.road_id: 235 | next_road_option = RoadOption.CHANGELANERIGHT 236 | next_segment = self._localize(next_waypoint.transform.location) 237 | if next_segment is not None: 238 | self._graph.add_edge( 239 | self._id_map[segment['entryxyz']], next_segment[0], entry_waypoint=waypoint, 240 | exit_waypoint=next_waypoint, intersection=False, exit_vector=None, 241 | path=[], length=0, type=next_road_option, change_waypoint=next_waypoint) 242 | right_found = True 243 | if waypoint.left_lane_marking and waypoint.left_lane_marking.lane_change & carla.LaneChange.Left and not left_found: 244 | next_waypoint = waypoint.get_left_lane() 245 | if next_waypoint is not None \ 246 | and next_waypoint.lane_type == carla.LaneType.Driving \ 247 | and waypoint.road_id == next_waypoint.road_id: 248 | next_road_option = RoadOption.CHANGELANELEFT 249 | next_segment = self._localize(next_waypoint.transform.location) 250 | if next_segment is not None: 251 | self._graph.add_edge( 252 | self._id_map[segment['entryxyz']], next_segment[0], entry_waypoint=waypoint, 253 | exit_waypoint=next_waypoint, intersection=False, exit_vector=None, 254 | path=[], length=0, type=next_road_option, change_waypoint=next_waypoint) 255 | left_found = True 256 | if left_found and right_found: 257 | break 258 | 259 | def _localize(self, location): 260 | """ 261 | This function finds the road segment that a given location 262 | is part of, returning the edge it belongs to 263 | """ 264 | waypoint = self._wmap.get_waypoint(location) 265 | edge = None 266 | try: 267 | edge = self._road_id_to_edge[waypoint.road_id][waypoint.section_id][waypoint.lane_id] 268 | except KeyError: 269 | pass 270 | return edge 271 | 272 | def _distance_heuristic(self, n1, n2): 273 | """ 274 | Distance heuristic calculator for path searching 275 | in self._graph 276 | """ 277 | l1 = np.array(self._graph.nodes[n1]['vertex']) 278 | l2 = np.array(self._graph.nodes[n2]['vertex']) 279 | return np.linalg.norm(l1-l2) 280 | 281 | def _path_search(self, origin, destination): 282 | """ 283 | This function finds the shortest path connecting origin and destination 284 | using A* search with distance heuristic. 285 | origin : carla.Location object of start position 286 | destination : carla.Location object of of end position 287 | return : path as list of node ids (as int) of the graph self._graph 288 | connecting origin and destination 289 | """ 290 | start, end = self._localize(origin), self._localize(destination) 291 | 292 | route = nx.astar_path( 293 | self._graph, source=start[0], target=end[0], 294 | heuristic=self._distance_heuristic, weight='length') 295 | route.append(end[1]) 296 | return route 297 | 298 | def _successive_last_intersection_edge(self, index, route): 299 | """ 300 | This method returns the last successive intersection edge 301 | from a starting index on the route. 302 | This helps moving past tiny intersection edges to calculate 303 | proper turn decisions. 304 | """ 305 | 306 | last_intersection_edge = None 307 | last_node = None 308 | for node1, node2 in [(route[i], route[i+1]) for i in range(index, len(route)-1)]: 309 | candidate_edge = self._graph.edges[node1, node2] 310 | if node1 == route[index]: 311 | last_intersection_edge = candidate_edge 312 | if candidate_edge['type'] == RoadOption.LANEFOLLOW and candidate_edge['intersection']: 313 | last_intersection_edge = candidate_edge 314 | last_node = node2 315 | else: 316 | break 317 | 318 | return last_node, last_intersection_edge 319 | 320 | def _turn_decision(self, index, route, threshold=math.radians(35)): 321 | """ 322 | This method returns the turn decision (RoadOption) for pair of edges 323 | around current index of route list 324 | """ 325 | 326 | decision = None 327 | previous_node = route[index-1] 328 | current_node = route[index] 329 | next_node = route[index+1] 330 | next_edge = self._graph.edges[current_node, next_node] 331 | if index > 0: 332 | if self._previous_decision != RoadOption.VOID \ 333 | and self._intersection_end_node > 0 \ 334 | and self._intersection_end_node != previous_node \ 335 | and next_edge['type'] == RoadOption.LANEFOLLOW \ 336 | and next_edge['intersection']: 337 | decision = self._previous_decision 338 | else: 339 | self._intersection_end_node = -1 340 | current_edge = self._graph.edges[previous_node, current_node] 341 | calculate_turn = current_edge['type'] == RoadOption.LANEFOLLOW and not current_edge[ 342 | 'intersection'] and next_edge['type'] == RoadOption.LANEFOLLOW and next_edge['intersection'] 343 | if calculate_turn: 344 | last_node, tail_edge = self._successive_last_intersection_edge(index, route) 345 | self._intersection_end_node = last_node 346 | if tail_edge is not None: 347 | next_edge = tail_edge 348 | cv, nv = current_edge['exit_vector'], next_edge['exit_vector'] 349 | if cv is None or nv is None: 350 | return next_edge['type'] 351 | cross_list = [] 352 | for neighbor in self._graph.successors(current_node): 353 | select_edge = self._graph.edges[current_node, neighbor] 354 | if select_edge['type'] == RoadOption.LANEFOLLOW: 355 | if neighbor != route[index+1]: 356 | sv = select_edge['net_vector'] 357 | cross_list.append(np.cross(cv, sv)[2]) 358 | next_cross = np.cross(cv, nv)[2] 359 | deviation = math.acos(np.clip( 360 | np.dot(cv, nv)/(np.linalg.norm(cv)*np.linalg.norm(nv)), -1.0, 1.0)) 361 | if not cross_list: 362 | cross_list.append(0) 363 | if deviation < threshold: 364 | decision = RoadOption.STRAIGHT 365 | elif cross_list and next_cross < min(cross_list): 366 | decision = RoadOption.LEFT 367 | elif cross_list and next_cross > max(cross_list): 368 | decision = RoadOption.RIGHT 369 | elif next_cross < 0: 370 | decision = RoadOption.LEFT 371 | elif next_cross > 0: 372 | decision = RoadOption.RIGHT 373 | else: 374 | decision = next_edge['type'] 375 | 376 | else: 377 | decision = next_edge['type'] 378 | 379 | self._previous_decision = decision 380 | return decision 381 | 382 | def _find_closest_in_list(self, current_waypoint, waypoint_list): 383 | min_distance = float('inf') 384 | closest_index = -1 385 | for i, waypoint in enumerate(waypoint_list): 386 | distance = waypoint.transform.location.distance( 387 | current_waypoint.transform.location) 388 | if distance < min_distance: 389 | min_distance = distance 390 | closest_index = i 391 | 392 | return closest_index 393 | -------------------------------------------------------------------------------- /Carla/retrive_data/agents/navigation/local_planner.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) # Copyright (c) 2018-2020 CVC. 2 | # 3 | # This work is licensed under the terms of the MIT license. 4 | # For a copy, see . 5 | 6 | """ This module contains a local planner to perform low-level waypoint following based on PID controllers. """ 7 | 8 | from enum import Enum 9 | from collections import deque 10 | import random 11 | 12 | import carla 13 | from agents.navigation.controller import VehiclePIDController 14 | from agents.tools.misc import draw_waypoints, get_speed 15 | 16 | 17 | class RoadOption(Enum): 18 | """ 19 | RoadOption represents the possible topological configurations when moving from a segment of lane to other. 20 | 21 | """ 22 | VOID = -1 23 | LEFT = 1 24 | RIGHT = 2 25 | STRAIGHT = 3 26 | LANEFOLLOW = 4 27 | CHANGELANELEFT = 5 28 | CHANGELANERIGHT = 6 29 | 30 | 31 | class LocalPlanner(object): 32 | """ 33 | LocalPlanner implements the basic behavior of following a 34 | trajectory of waypoints that is generated on-the-fly. 35 | 36 | The low-level motion of the vehicle is computed by using two PID controllers, 37 | one is used for the lateral control and the other for the longitudinal control (cruise speed). 38 | 39 | When multiple paths are available (intersections) this local planner makes a random choice, 40 | unless a given global plan has already been specified. 41 | """ 42 | 43 | def __init__(self, vehicle, opt_dict={}): 44 | """ 45 | :param vehicle: actor to apply to local planner logic onto 46 | :param opt_dict: dictionary of arguments with different parameters: 47 | dt: time between simulation steps 48 | target_speed: desired cruise speed in Km/h 49 | sampling_radius: distance between the waypoints part of the plan 50 | lateral_control_dict: values of the lateral PID controller 51 | longitudinal_control_dict: values of the longitudinal PID controller 52 | max_throttle: maximum throttle applied to the vehicle 53 | max_brake: maximum brake applied to the vehicle 54 | max_steering: maximum steering applied to the vehicle 55 | offset: distance between the route waypoints and the center of the lane 56 | """ 57 | self._vehicle = vehicle 58 | self._world = self._vehicle.get_world() 59 | self._map = self._world.get_map() 60 | 61 | self._vehicle_controller = None 62 | self.target_waypoint = None 63 | self.target_road_option = None 64 | 65 | self._waypoints_queue = deque(maxlen=10000) 66 | self._min_waypoint_queue_length = 100 67 | self._stop_waypoint_creation = False 68 | 69 | # Base parameters 70 | self._dt = 1.0 / 20.0 71 | self._target_speed = 20.0 # Km/h 72 | self._sampling_radius = 2.0 73 | self._args_lateral_dict = {'K_P': 1.95, 'K_I': 0.05, 'K_D': 0.2, 'dt': self._dt} 74 | self._args_longitudinal_dict = {'K_P': 1.0, 'K_I': 0.05, 'K_D': 0, 'dt': self._dt} 75 | self._max_throt = 0.75 76 | self._max_brake = 0.3 77 | self._max_steer = 0.8 78 | self._offset = 0 79 | self._base_min_distance = 3.0 80 | self._follow_speed_limits = False 81 | 82 | # Overload parameters 83 | if opt_dict: 84 | if 'dt' in opt_dict: 85 | self._dt = opt_dict['dt'] 86 | if 'target_speed' in opt_dict: 87 | self._target_speed = opt_dict['target_speed'] 88 | if 'sampling_radius' in opt_dict: 89 | self._sampling_radius = opt_dict['sampling_radius'] 90 | if 'lateral_control_dict' in opt_dict: 91 | self._args_lateral_dict = opt_dict['lateral_control_dict'] 92 | if 'longitudinal_control_dict' in opt_dict: 93 | self._args_longitudinal_dict = opt_dict['longitudinal_control_dict'] 94 | if 'max_throttle' in opt_dict: 95 | self._max_throt = opt_dict['max_throttle'] 96 | if 'max_brake' in opt_dict: 97 | self._max_brake = opt_dict['max_brake'] 98 | if 'max_steering' in opt_dict: 99 | self._max_steer = opt_dict['max_steering'] 100 | if 'offset' in opt_dict: 101 | self._offset = opt_dict['offset'] 102 | if 'base_min_distance' in opt_dict: 103 | self._base_min_distance = opt_dict['base_min_distance'] 104 | if 'follow_speed_limits' in opt_dict: 105 | self._follow_speed_limits = opt_dict['follow_speed_limits'] 106 | 107 | # initializing controller 108 | self._init_controller() 109 | 110 | def reset_vehicle(self): 111 | """Reset the ego-vehicle""" 112 | self._vehicle = None 113 | 114 | def _init_controller(self): 115 | """Controller initialization""" 116 | self._vehicle_controller = VehiclePIDController(self._vehicle, 117 | args_lateral=self._args_lateral_dict, 118 | args_longitudinal=self._args_longitudinal_dict, 119 | offset=self._offset, 120 | max_throttle=self._max_throt, 121 | max_brake=self._max_brake, 122 | max_steering=self._max_steer) 123 | 124 | # Compute the current vehicle waypoint 125 | current_waypoint = self._map.get_waypoint(self._vehicle.get_location()) 126 | self.target_waypoint, self.target_road_option = (current_waypoint, RoadOption.LANEFOLLOW) 127 | self._waypoints_queue.append((self.target_waypoint, self.target_road_option)) 128 | 129 | def set_speed(self, speed): 130 | """ 131 | Changes the target speed 132 | 133 | :param speed: new target speed in Km/h 134 | :return: 135 | """ 136 | if self._follow_speed_limits: 137 | print("WARNING: The max speed is currently set to follow the speed limits. " 138 | "Use 'follow_speed_limits' to deactivate this") 139 | self._target_speed = speed 140 | 141 | def follow_speed_limits(self, value=True): 142 | """ 143 | Activates a flag that makes the max speed dynamically vary according to the spped limits 144 | 145 | :param value: bool 146 | :return: 147 | """ 148 | self._follow_speed_limits = value 149 | 150 | def _compute_next_waypoints(self, k=1): 151 | """ 152 | Add new waypoints to the trajectory queue. 153 | 154 | :param k: how many waypoints to compute 155 | :return: 156 | """ 157 | # check we do not overflow the queue 158 | available_entries = self._waypoints_queue.maxlen - len(self._waypoints_queue) 159 | k = min(available_entries, k) 160 | 161 | for _ in range(k): 162 | last_waypoint = self._waypoints_queue[-1][0] 163 | next_waypoints = list(last_waypoint.next(self._sampling_radius)) 164 | 165 | if len(next_waypoints) == 0: 166 | break 167 | elif len(next_waypoints) == 1: 168 | # only one option available ==> lanefollowing 169 | next_waypoint = next_waypoints[0] 170 | road_option = RoadOption.LANEFOLLOW 171 | else: 172 | # random choice between the possible options 173 | road_options_list = _retrieve_options( 174 | next_waypoints, last_waypoint) 175 | road_option = random.choice(road_options_list) 176 | next_waypoint = next_waypoints[road_options_list.index( 177 | road_option)] 178 | 179 | self._waypoints_queue.append((next_waypoint, road_option)) 180 | 181 | def set_global_plan(self, current_plan, stop_waypoint_creation=True, clean_queue=True): 182 | """ 183 | Adds a new plan to the local planner. A plan must be a list of [carla.Waypoint, RoadOption] pairs 184 | The 'clean_queue` parameter erases the previous plan if True, otherwise, it adds it to the old one 185 | The 'stop_waypoint_creation' flag stops the automatic creation of random waypoints 186 | 187 | :param current_plan: list of (carla.Waypoint, RoadOption) 188 | :param stop_waypoint_creation: bool 189 | :param clean_queue: bool 190 | :return: 191 | """ 192 | if clean_queue: 193 | self._waypoints_queue.clear() 194 | 195 | # Remake the waypoints queue if the new plan has a higher length than the queue 196 | new_plan_length = len(current_plan) + len(self._waypoints_queue) 197 | if new_plan_length > self._waypoints_queue.maxlen: 198 | new_waypoint_queue = deque(maxlen=new_plan_length) 199 | for wp in self._waypoints_queue: 200 | new_waypoint_queue.append(wp) 201 | self._waypoints_queue = new_waypoint_queue 202 | 203 | for elem in current_plan: 204 | self._waypoints_queue.append(elem) 205 | 206 | self._stop_waypoint_creation = stop_waypoint_creation 207 | 208 | def run_step(self, debug=False): 209 | """ 210 | Execute one step of local planning which involves running the longitudinal and lateral PID controllers to 211 | follow the waypoints trajectory. 212 | 213 | :param debug: boolean flag to activate waypoints debugging 214 | :return: control to be applied 215 | """ 216 | if self._follow_speed_limits: 217 | self._target_speed = self._vehicle.get_speed_limit() 218 | 219 | # Add more waypoints too few in the horizon 220 | if not self._stop_waypoint_creation and len(self._waypoints_queue) < self._min_waypoint_queue_length: 221 | self._compute_next_waypoints(k=self._min_waypoint_queue_length) 222 | 223 | # Purge the queue of obsolete waypoints 224 | veh_location = self._vehicle.get_location() 225 | vehicle_speed = get_speed(self._vehicle) / 3.6 226 | self._min_distance = self._base_min_distance + 0.5 *vehicle_speed 227 | 228 | num_waypoint_removed = 0 229 | for waypoint, _ in self._waypoints_queue: 230 | 231 | if len(self._waypoints_queue) - num_waypoint_removed == 1: 232 | min_distance = 1 # Don't remove the last waypoint until very close by 233 | else: 234 | min_distance = self._min_distance 235 | 236 | if veh_location.distance(waypoint.transform.location) < min_distance: 237 | num_waypoint_removed += 1 238 | else: 239 | break 240 | 241 | if num_waypoint_removed > 0: 242 | for _ in range(num_waypoint_removed): 243 | self._waypoints_queue.popleft() 244 | 245 | # Get the target waypoint and move using the PID controllers. Stop if no target waypoint 246 | if len(self._waypoints_queue) == 0: 247 | control = carla.VehicleControl() 248 | control.steer = 0.0 249 | control.throttle = 0.0 250 | control.brake = 1.0 251 | control.hand_brake = False 252 | control.manual_gear_shift = False 253 | else: 254 | self.target_waypoint, self.target_road_option = self._waypoints_queue[0] 255 | control = self._vehicle_controller.run_step(self._target_speed, self.target_waypoint) 256 | 257 | if debug: 258 | draw_waypoints(self._vehicle.get_world(), [self.target_waypoint], 1.0) 259 | 260 | return control 261 | 262 | def get_incoming_waypoint_and_direction(self, steps=3): 263 | """ 264 | Returns direction and waypoint at a distance ahead defined by the user. 265 | 266 | :param steps: number of steps to get the incoming waypoint. 267 | """ 268 | if len(self._waypoints_queue) > steps: 269 | return self._waypoints_queue[steps] 270 | 271 | else: 272 | try: 273 | wpt, direction = self._waypoints_queue[-1] 274 | return wpt, direction 275 | except IndexError as i: 276 | return None, RoadOption.VOID 277 | 278 | def get_plan(self): 279 | """Returns the current plan of the local planner""" 280 | return self._waypoints_queue 281 | 282 | def done(self): 283 | """ 284 | Returns whether or not the planner has finished 285 | 286 | :return: boolean 287 | """ 288 | return len(self._waypoints_queue) == 0 289 | 290 | 291 | def _retrieve_options(list_waypoints, current_waypoint): 292 | """ 293 | Compute the type of connection between the current active waypoint and the multiple waypoints present in 294 | list_waypoints. The result is encoded as a list of RoadOption enums. 295 | 296 | :param list_waypoints: list with the possible target waypoints in case of multiple options 297 | :param current_waypoint: current active waypoint 298 | :return: list of RoadOption enums representing the type of connection from the active waypoint to each 299 | candidate in list_waypoints 300 | """ 301 | options = [] 302 | for next_waypoint in list_waypoints: 303 | # this is needed because something we are linking to 304 | # the beggining of an intersection, therefore the 305 | # variation in angle is small 306 | next_next_waypoint = next_waypoint.next(3.0)[0] 307 | link = _compute_connection(current_waypoint, next_next_waypoint) 308 | options.append(link) 309 | 310 | return options 311 | 312 | 313 | def _compute_connection(current_waypoint, next_waypoint, threshold=35): 314 | """ 315 | Compute the type of topological connection between an active waypoint (current_waypoint) and a target waypoint 316 | (next_waypoint). 317 | 318 | :param current_waypoint: active waypoint 319 | :param next_waypoint: target waypoint 320 | :return: the type of topological connection encoded as a RoadOption enum: 321 | RoadOption.STRAIGHT 322 | RoadOption.LEFT 323 | RoadOption.RIGHT 324 | """ 325 | n = next_waypoint.transform.rotation.yaw 326 | n = n % 360.0 327 | 328 | c = current_waypoint.transform.rotation.yaw 329 | c = c % 360.0 330 | 331 | diff_angle = (n - c) % 180.0 332 | if diff_angle < threshold or diff_angle > (180 - threshold): 333 | return RoadOption.STRAIGHT 334 | elif diff_angle > 90.0: 335 | return RoadOption.LEFT 336 | else: 337 | return RoadOption.RIGHT 338 | -------------------------------------------------------------------------------- /Carla/retrive_data/agents/tools/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TroddenSpade/Flexible-Conditional-Imitation-Learning/7ce40ffeac69f3ed09afa3523ef32adb91ad5355/Carla/retrive_data/agents/tools/__init__.py -------------------------------------------------------------------------------- /Carla/retrive_data/agents/tools/misc.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2018 Intel Labs. 4 | # authors: German Ros (german.ros@intel.com) 5 | # 6 | # This work is licensed under the terms of the MIT license. 7 | # For a copy, see . 8 | 9 | """ Module with auxiliary functions. """ 10 | 11 | import math 12 | import numpy as np 13 | import carla 14 | 15 | def draw_waypoints(world, waypoints, z=0.5): 16 | """ 17 | Draw a list of waypoints at a certain height given in z. 18 | 19 | :param world: carla.world object 20 | :param waypoints: list or iterable container with the waypoints to draw 21 | :param z: height in meters 22 | """ 23 | for wpt in waypoints: 24 | wpt_t = wpt.transform 25 | begin = wpt_t.location + carla.Location(z=z) 26 | angle = math.radians(wpt_t.rotation.yaw) 27 | end = begin + carla.Location(x=math.cos(angle), y=math.sin(angle)) 28 | world.debug.draw_arrow(begin, end, arrow_size=0.3, life_time=1.0) 29 | 30 | 31 | def get_speed(vehicle): 32 | """ 33 | Compute speed of a vehicle in Km/h. 34 | 35 | :param vehicle: the vehicle for which speed is calculated 36 | :return: speed as a float in Km/h 37 | """ 38 | vel = vehicle.get_velocity() 39 | 40 | return 3.6 * math.sqrt(vel.x ** 2 + vel.y ** 2 + vel.z ** 2) 41 | 42 | def get_trafficlight_trigger_location(traffic_light): 43 | """ 44 | Calculates the yaw of the waypoint that represents the trigger volume of the traffic light 45 | """ 46 | def rotate_point(point, radians): 47 | """ 48 | rotate a given point by a given angle 49 | """ 50 | rotated_x = math.cos(radians) * point.x - math.sin(radians) * point.y 51 | rotated_y = math.sin(radians) * point.x - math.cos(radians) * point.y 52 | 53 | return carla.Vector3D(rotated_x, rotated_y, point.z) 54 | 55 | base_transform = traffic_light.get_transform() 56 | base_rot = base_transform.rotation.yaw 57 | area_loc = base_transform.transform(traffic_light.trigger_volume.location) 58 | area_ext = traffic_light.trigger_volume.extent 59 | 60 | point = rotate_point(carla.Vector3D(0, 0, area_ext.z), math.radians(base_rot)) 61 | point_location = area_loc + carla.Location(x=point.x, y=point.y) 62 | 63 | return carla.Location(point_location.x, point_location.y, point_location.z) 64 | 65 | 66 | def is_within_distance(target_transform, reference_transform, max_distance, angle_interval=None): 67 | """ 68 | Check if a location is both within a certain distance from a reference object. 69 | By using 'angle_interval', the angle between the location and reference transform 70 | will also be tkaen into account, being 0 a location in front and 180, one behind. 71 | 72 | :param target_transform: location of the target object 73 | :param reference_transform: location of the reference object 74 | :param max_distance: maximum allowed distance 75 | :param angle_interval: only locations between [min, max] angles will be considered. This isn't checked by default. 76 | :return: boolean 77 | """ 78 | target_vector = np.array([ 79 | target_transform.location.x - reference_transform.location.x, 80 | target_transform.location.y - reference_transform.location.y 81 | ]) 82 | norm_target = np.linalg.norm(target_vector) 83 | 84 | # If the vector is too short, we can simply stop here 85 | if norm_target < 0.001: 86 | return True 87 | 88 | # Further than the max distance 89 | if norm_target > max_distance: 90 | return False 91 | 92 | # We don't care about the angle, nothing else to check 93 | if not angle_interval: 94 | return True 95 | 96 | min_angle = angle_interval[0] 97 | max_angle = angle_interval[1] 98 | 99 | fwd = reference_transform.get_forward_vector() 100 | forward_vector = np.array([fwd.x, fwd.y]) 101 | angle = math.degrees(math.acos(np.clip(np.dot(forward_vector, target_vector) / norm_target, -1., 1.))) 102 | 103 | return min_angle < angle < max_angle 104 | 105 | 106 | def compute_magnitude_angle(target_location, current_location, orientation): 107 | """ 108 | Compute relative angle and distance between a target_location and a current_location 109 | 110 | :param target_location: location of the target object 111 | :param current_location: location of the reference object 112 | :param orientation: orientation of the reference object 113 | :return: a tuple composed by the distance to the object and the angle between both objects 114 | """ 115 | target_vector = np.array([target_location.x - current_location.x, target_location.y - current_location.y]) 116 | norm_target = np.linalg.norm(target_vector) 117 | 118 | forward_vector = np.array([math.cos(math.radians(orientation)), math.sin(math.radians(orientation))]) 119 | d_angle = math.degrees(math.acos(np.clip(np.dot(forward_vector, target_vector) / norm_target, -1., 1.))) 120 | 121 | return (norm_target, d_angle) 122 | 123 | 124 | def distance_vehicle(waypoint, vehicle_transform): 125 | """ 126 | Returns the 2D distance from a waypoint to a vehicle 127 | 128 | :param waypoint: actual waypoint 129 | :param vehicle_transform: transform of the target vehicle 130 | """ 131 | loc = vehicle_transform.location 132 | x = waypoint.transform.location.x - loc.x 133 | y = waypoint.transform.location.y - loc.y 134 | 135 | return math.sqrt(x * x + y * y) 136 | 137 | 138 | def vector(location_1, location_2): 139 | """ 140 | Returns the unit vector from location_1 to location_2 141 | 142 | :param location_1, location_2: carla.Location objects 143 | """ 144 | x = location_2.x - location_1.x 145 | y = location_2.y - location_1.y 146 | z = location_2.z - location_1.z 147 | norm = np.linalg.norm([x, y, z]) + np.finfo(float).eps 148 | 149 | return [x / norm, y / norm, z / norm] 150 | 151 | 152 | def compute_distance(location_1, location_2): 153 | """ 154 | Euclidean distance between 3D points 155 | 156 | :param location_1, location_2: 3D points 157 | """ 158 | x = location_2.x - location_1.x 159 | y = location_2.y - location_1.y 160 | z = location_2.z - location_1.z 161 | norm = np.linalg.norm([x, y, z]) + np.finfo(float).eps 162 | return norm 163 | 164 | 165 | def positive(num): 166 | """ 167 | Return the given number if positive, else 0 168 | 169 | :param num: value to check 170 | """ 171 | return num if num > 0.0 else 0.0 172 | -------------------------------------------------------------------------------- /Carla/retrive_data/generator.py: -------------------------------------------------------------------------------- 1 | import time 2 | 3 | import carla 4 | from numpy import random 5 | 6 | import logging 7 | 8 | def generate_ego_vehicle(world): 9 | ego_bp = world.get_blueprint_library().find('vehicle.tesla.model3') 10 | ego_bp.set_attribute('role_name','ego') 11 | ego_color = random.choice(ego_bp.get_attribute('color').recommended_values) 12 | ego_bp.set_attribute('color',ego_color) 13 | 14 | spawn_points = world.get_map().get_spawn_points() 15 | number_of_spawn_points = len(spawn_points) 16 | 17 | if 0 < number_of_spawn_points: 18 | random.shuffle(spawn_points) 19 | ego_transform = spawn_points[0] 20 | ego_vehicle = world.spawn_actor(ego_bp,ego_transform) 21 | print('\nVehicle is spawned') 22 | else: 23 | print('\nCould not found any spawn points!') 24 | 25 | assert ego_vehicle != None 26 | return ego_vehicle 27 | 28 | 29 | def get_actor_blueprints(world, filter, generation): 30 | bps = world.get_blueprint_library().filter(filter) 31 | 32 | if generation.lower() == "all": 33 | return bps 34 | 35 | # If the filter returns only one bp, we assume that this one needed 36 | # and therefore, we ignore the generation 37 | if len(bps) == 1: 38 | return bps 39 | 40 | try: 41 | int_generation = int(generation) 42 | # Check if generation is in available generations 43 | if int_generation in [1, 2]: 44 | bps = [x for x in bps if int(x.get_attribute('generation')) == int_generation] 45 | return bps 46 | else: 47 | print(" Warning! Actor Generation is not valid. No actor will be spawned.") 48 | return [] 49 | except: 50 | print(" Warning! Actor Generation is not valid. No actor will be spawned.") 51 | return [] 52 | 53 | 54 | def generate_traffic(client, world): 55 | logging.basicConfig(format='%(levelname)s: %(message)s', level=logging.INFO) 56 | 57 | vehicles_list = [] 58 | walkers_list = [] 59 | all_id = [] 60 | 61 | synchronous_master = False 62 | random.seed(int(time.time())) 63 | 64 | traffic_manager = client.get_trafficmanager(8000) 65 | traffic_manager.set_global_distance_to_leading_vehicle(2.5) 66 | traffic_manager.set_hybrid_physics_mode(True) 67 | traffic_manager.set_hybrid_physics_radius(70.0) 68 | 69 | # settings = world.get_settings() 70 | # world.apply_settings(settings) 71 | 72 | print("You are currently in asynchronous mode. If this is a traffic simulation, \ 73 | you could experience some issues. If it's not working correctly, switch to synchronous \ 74 | mode by using traffic_manager.set_synchronous_mode(True)") 75 | 76 | 77 | blueprintsWalkers = get_actor_blueprints(world, 'walker.pedestrian.*', '2') 78 | 79 | blueprints = get_actor_blueprints(world, 'vehicle.*', 'All') 80 | blueprints = [x for x in blueprints if int(x.get_attribute('number_of_wheels')) == 4] 81 | blueprints = [x for x in blueprints if not x.id.endswith('microlino')] 82 | blueprints = [x for x in blueprints if not x.id.endswith('carlacola')] 83 | blueprints = [x for x in blueprints if not x.id.endswith('cybertruck')] 84 | blueprints = [x for x in blueprints if not x.id.endswith('t2')] 85 | blueprints = [x for x in blueprints if not x.id.endswith('sprinter')] 86 | blueprints = [x for x in blueprints if not x.id.endswith('firetruck')] 87 | blueprints = [x for x in blueprints if not x.id.endswith('ambulance')] 88 | 89 | blueprints = sorted(blueprints, key=lambda bp: bp.id) 90 | 91 | spawn_points = world.get_map().get_spawn_points() 92 | number_of_spawn_points = len(spawn_points) 93 | number_of_vehicles = 100 94 | 95 | if number_of_vehicles < number_of_spawn_points: 96 | random.shuffle(spawn_points) 97 | elif number_of_vehicles > number_of_spawn_points: 98 | msg = 'requested %d vehicles, but could only find %d spawn points' 99 | logging.warning(msg, number_of_vehicles, number_of_spawn_points) 100 | number_of_vehicles = number_of_spawn_points 101 | 102 | # @todo cannot import these directly. 103 | SpawnActor = carla.command.SpawnActor 104 | SetAutopilot = carla.command.SetAutopilot 105 | FutureActor = carla.command.FutureActor 106 | 107 | # -------------- 108 | # Spawn vehicles 109 | # -------------- 110 | batch = [] 111 | for n, transform in enumerate(spawn_points): 112 | if n >= number_of_vehicles: 113 | break 114 | blueprint = random.choice(blueprints) 115 | if blueprint.has_attribute('color'): 116 | color = random.choice(blueprint.get_attribute('color').recommended_values) 117 | blueprint.set_attribute('color', color) 118 | if blueprint.has_attribute('driver_id'): 119 | driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values) 120 | blueprint.set_attribute('driver_id', driver_id) 121 | blueprint.set_attribute('role_name', 'autopilot') 122 | 123 | # spawn the cars and set their autopilot and light state all together 124 | batch.append(SpawnActor(blueprint, transform) 125 | .then(SetAutopilot(FutureActor, True, traffic_manager.get_port()))) 126 | 127 | for response in client.apply_batch_sync(batch, synchronous_master): 128 | if response.error: 129 | logging.error(response.error) 130 | else: 131 | vehicles_list.append(response.actor_id) 132 | 133 | # ------------- 134 | # Spawn Walkers 135 | # ------------- 136 | # some settings 137 | percentagePedestriansRunning = 0.0 # how many pedestrians will run 138 | percentagePedestriansCrossing = 0.5 # how many pedestrians will walk through the road 139 | world.set_pedestrians_seed(0) 140 | random.seed(0) 141 | # 1. take all the random locations to spawn 142 | number_of_walkers = 10 143 | spawn_points = [] 144 | for i in range(number_of_walkers): 145 | spawn_point = carla.Transform() 146 | loc = world.get_random_location_from_navigation() 147 | if (loc != None): 148 | spawn_point.location = loc 149 | spawn_points.append(spawn_point) 150 | # 2. we spawn the walker object 151 | batch = [] 152 | walker_speed = [] 153 | for spawn_point in spawn_points: 154 | walker_bp = random.choice(blueprintsWalkers) 155 | # set as not invincible 156 | if walker_bp.has_attribute('is_invincible'): 157 | walker_bp.set_attribute('is_invincible', 'false') 158 | # set the max speed 159 | if walker_bp.has_attribute('speed'): 160 | if (random.random() > percentagePedestriansRunning): 161 | # walking 162 | walker_speed.append(walker_bp.get_attribute('speed').recommended_values[1]) 163 | else: 164 | # running 165 | walker_speed.append(walker_bp.get_attribute('speed').recommended_values[2]) 166 | else: 167 | print("Walker has no speed") 168 | walker_speed.append(0.0) 169 | batch.append(SpawnActor(walker_bp, spawn_point)) 170 | results = client.apply_batch_sync(batch, True) 171 | walker_speed2 = [] 172 | for i in range(len(results)): 173 | if results[i].error: 174 | logging.error(results[i].error) 175 | else: 176 | walkers_list.append({"id": results[i].actor_id}) 177 | walker_speed2.append(walker_speed[i]) 178 | walker_speed = walker_speed2 179 | # 3. we spawn the walker controller 180 | batch = [] 181 | walker_controller_bp = world.get_blueprint_library().find('controller.ai.walker') 182 | for i in range(len(walkers_list)): 183 | batch.append(SpawnActor(walker_controller_bp, carla.Transform(), walkers_list[i]["id"])) 184 | results = client.apply_batch_sync(batch, True) 185 | for i in range(len(results)): 186 | if results[i].error: 187 | logging.error(results[i].error) 188 | else: 189 | walkers_list[i]["con"] = results[i].actor_id 190 | # 4. we put together the walkers and controllers id to get the objects from their id 191 | for i in range(len(walkers_list)): 192 | all_id.append(walkers_list[i]["con"]) 193 | all_id.append(walkers_list[i]["id"]) 194 | all_actors = world.get_actors(all_id) 195 | 196 | # wait for a tick to ensure client receives the last transform of the walkers we have just created 197 | world.wait_for_tick() 198 | 199 | 200 | # 5. initialize each controller and set target to walk to (list is [controler, actor, controller, actor ...]) 201 | # set how many pedestrians can cross the road 202 | world.set_pedestrians_cross_factor(percentagePedestriansCrossing) 203 | for i in range(0, len(all_id), 2): 204 | # start walker 205 | all_actors[i].start() 206 | # set walk to random point 207 | all_actors[i].go_to_location(world.get_random_location_from_navigation()) 208 | # max speed 209 | all_actors[i].set_max_speed(float(walker_speed[int(i/2)])) 210 | 211 | print('spawned %d vehicles and %d walkers, press Ctrl+C to exit.' % (len(vehicles_list), len(walkers_list))) 212 | 213 | # Example of how to use Traffic Manager parameters 214 | traffic_manager.global_percentage_speed_difference(30.0) 215 | 216 | -------------------------------------------------------------------------------- /Carla/retrive_data/main.py: -------------------------------------------------------------------------------- 1 | #Dependencies 2 | from email import header 3 | import glob 4 | import os 5 | import sys 6 | import time 7 | import numpy as np 8 | import carla 9 | import logging 10 | import random 11 | import pandas as pd 12 | from datetime import datetime 13 | 14 | from agents.navigation.global_route_planner import GlobalRoutePlanner 15 | 16 | from generate_traffic import generate_traffic 17 | 18 | 19 | TOWN_NAME = 'Town01' 20 | DIRECTORY = 'Data' 21 | IMAGE_WIDTH = 88 22 | IMAGE_HEIGHT = 200 23 | RECORD_LENGTH = 2000 24 | 25 | 26 | def get_actor_blueprints(world, filter, generation): 27 | bps = world.get_blueprint_library().filter(filter) 28 | 29 | if generation.lower() == "all": 30 | return bps 31 | 32 | # If the filter returns only one bp, we assume that this one needed 33 | # and therefore, we ignore the generation 34 | if len(bps) == 1: 35 | return bps 36 | 37 | try: 38 | int_generation = int(generation) 39 | # Check if generation is in available generations 40 | if int_generation in [1, 2]: 41 | bps = [x for x in bps if int(x.get_attribute('generation')) == int_generation] 42 | return bps 43 | else: 44 | print(" Warning! Actor Generation is not valid. No actor will be spawned.") 45 | return [] 46 | except: 47 | print(" Warning! Actor Generation is not valid. No actor will be spawned.") 48 | return [] 49 | 50 | 51 | 52 | # datetime object containing current date and time 53 | now = datetime.now() 54 | # dd/mm/YY H:M:S 55 | dt_string = now.strftime("%Y-%m-%d_%H-%M-%S") 56 | print('\nStarted at =', dt_string) 57 | 58 | rec_dir = os.path.join(DIRECTORY, dt_string) 59 | image_dir = os.path.join(rec_dir, 'images') 60 | try: 61 | os.makedirs(image_dir) 62 | print("\n"+ rec_dir + ' created') 63 | except Exception as inst: 64 | print("\nError: ") 65 | print(inst) 66 | 67 | 68 | client = carla.Client("127.0.0.1", 2000) 69 | client.set_timeout(10.0) 70 | client.reload_world() 71 | 72 | world = client.load_world(TOWN_NAME) 73 | print('\n'+ TOWN_NAME + ' Loaded.') 74 | 75 | 76 | ego_bp = world.get_blueprint_library().find('vehicle.tesla.model3') 77 | ego_bp.set_attribute('role_name','ego') 78 | ego_color = random.choice(ego_bp.get_attribute('color').recommended_values) 79 | ego_bp.set_attribute('color',ego_color) 80 | 81 | spawn_points = world.get_map().get_spawn_points() 82 | number_of_spawn_points = len(spawn_points) 83 | 84 | if 0 < number_of_spawn_points: 85 | random.shuffle(spawn_points) 86 | ego_transform = spawn_points[0] 87 | ego_vehicle = world.spawn_actor(ego_bp,ego_transform) 88 | print('\nVehicle is spawned') 89 | else: 90 | print('\nCould not found any spawn points!') 91 | 92 | 93 | 94 | 95 | 96 | logging.basicConfig(format='%(levelname)s: %(message)s', level=logging.INFO) 97 | 98 | vehicles_list = [] 99 | walkers_list = [] 100 | all_id = [] 101 | 102 | synchronous_master = False 103 | random.seed(int(time.time())) 104 | 105 | traffic_manager = client.get_trafficmanager(8000) 106 | traffic_manager.set_global_distance_to_leading_vehicle(2.5) 107 | traffic_manager.set_hybrid_physics_mode(True) 108 | traffic_manager.set_hybrid_physics_radius(70.0) 109 | 110 | # settings = world.get_settings() 111 | # world.apply_settings(settings) 112 | 113 | print("You are currently in asynchronous mode. If this is a traffic simulation, \ 114 | you could experience some issues. If it's not working correctly, switch to synchronous \ 115 | mode by using traffic_manager.set_synchronous_mode(True)") 116 | 117 | 118 | blueprintsWalkers = get_actor_blueprints(world, 'walker.pedestrian.*', '2') 119 | 120 | blueprints = get_actor_blueprints(world, 'vehicle.*', 'All') 121 | blueprints = [x for x in blueprints if int(x.get_attribute('number_of_wheels')) == 4] 122 | blueprints = [x for x in blueprints if not x.id.endswith('microlino')] 123 | blueprints = [x for x in blueprints if not x.id.endswith('carlacola')] 124 | blueprints = [x for x in blueprints if not x.id.endswith('cybertruck')] 125 | blueprints = [x for x in blueprints if not x.id.endswith('t2')] 126 | blueprints = [x for x in blueprints if not x.id.endswith('sprinter')] 127 | blueprints = [x for x in blueprints if not x.id.endswith('firetruck')] 128 | blueprints = [x for x in blueprints if not x.id.endswith('ambulance')] 129 | 130 | blueprints = sorted(blueprints, key=lambda bp: bp.id) 131 | 132 | spawn_points = world.get_map().get_spawn_points() 133 | number_of_spawn_points = len(spawn_points) 134 | number_of_vehicles = 100 135 | 136 | if number_of_vehicles < number_of_spawn_points: 137 | random.shuffle(spawn_points) 138 | elif number_of_vehicles > number_of_spawn_points: 139 | msg = 'requested %d vehicles, but could only find %d spawn points' 140 | logging.warning(msg, number_of_vehicles, number_of_spawn_points) 141 | number_of_vehicles = number_of_spawn_points 142 | 143 | # @todo cannot import these directly. 144 | SpawnActor = carla.command.SpawnActor 145 | SetAutopilot = carla.command.SetAutopilot 146 | FutureActor = carla.command.FutureActor 147 | 148 | # -------------- 149 | # Spawn vehicles 150 | # -------------- 151 | batch = [] 152 | for n, transform in enumerate(spawn_points): 153 | if n >= number_of_vehicles: 154 | break 155 | blueprint = random.choice(blueprints) 156 | if blueprint.has_attribute('color'): 157 | color = random.choice(blueprint.get_attribute('color').recommended_values) 158 | blueprint.set_attribute('color', color) 159 | if blueprint.has_attribute('driver_id'): 160 | driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values) 161 | blueprint.set_attribute('driver_id', driver_id) 162 | blueprint.set_attribute('role_name', 'autopilot') 163 | 164 | # spawn the cars and set their autopilot and light state all together 165 | batch.append(SpawnActor(blueprint, transform) 166 | .then(SetAutopilot(FutureActor, True, traffic_manager.get_port()))) 167 | 168 | for response in client.apply_batch_sync(batch, synchronous_master): 169 | if response.error: 170 | logging.error(response.error) 171 | else: 172 | vehicles_list.append(response.actor_id) 173 | 174 | # ------------- 175 | # Spawn Walkers 176 | # ------------- 177 | # some settings 178 | percentagePedestriansRunning = 0.0 # how many pedestrians will run 179 | percentagePedestriansCrossing = 0.5 # how many pedestrians will walk through the road 180 | world.set_pedestrians_seed(0) 181 | random.seed(0) 182 | # 1. take all the random locations to spawn 183 | number_of_walkers = 10 184 | spawn_points = [] 185 | for i in range(number_of_walkers): 186 | spawn_point = carla.Transform() 187 | loc = world.get_random_location_from_navigation() 188 | if (loc != None): 189 | spawn_point.location = loc 190 | spawn_points.append(spawn_point) 191 | # 2. we spawn the walker object 192 | batch = [] 193 | walker_speed = [] 194 | for spawn_point in spawn_points: 195 | walker_bp = random.choice(blueprintsWalkers) 196 | # set as not invincible 197 | if walker_bp.has_attribute('is_invincible'): 198 | walker_bp.set_attribute('is_invincible', 'false') 199 | # set the max speed 200 | if walker_bp.has_attribute('speed'): 201 | if (random.random() > percentagePedestriansRunning): 202 | # walking 203 | walker_speed.append(walker_bp.get_attribute('speed').recommended_values[1]) 204 | else: 205 | # running 206 | walker_speed.append(walker_bp.get_attribute('speed').recommended_values[2]) 207 | else: 208 | print("Walker has no speed") 209 | walker_speed.append(0.0) 210 | batch.append(SpawnActor(walker_bp, spawn_point)) 211 | results = client.apply_batch_sync(batch, True) 212 | walker_speed2 = [] 213 | for i in range(len(results)): 214 | if results[i].error: 215 | logging.error(results[i].error) 216 | else: 217 | walkers_list.append({"id": results[i].actor_id}) 218 | walker_speed2.append(walker_speed[i]) 219 | walker_speed = walker_speed2 220 | # 3. we spawn the walker controller 221 | batch = [] 222 | walker_controller_bp = world.get_blueprint_library().find('controller.ai.walker') 223 | for i in range(len(walkers_list)): 224 | batch.append(SpawnActor(walker_controller_bp, carla.Transform(), walkers_list[i]["id"])) 225 | results = client.apply_batch_sync(batch, True) 226 | for i in range(len(results)): 227 | if results[i].error: 228 | logging.error(results[i].error) 229 | else: 230 | walkers_list[i]["con"] = results[i].actor_id 231 | # 4. we put together the walkers and controllers id to get the objects from their id 232 | for i in range(len(walkers_list)): 233 | all_id.append(walkers_list[i]["con"]) 234 | all_id.append(walkers_list[i]["id"]) 235 | all_actors = world.get_actors(all_id) 236 | 237 | # wait for a tick to ensure client receives the last transform of the walkers we have just created 238 | world.wait_for_tick() 239 | 240 | 241 | # 5. initialize each controller and set target to walk to (list is [controler, actor, controller, actor ...]) 242 | # set how many pedestrians can cross the road 243 | world.set_pedestrians_cross_factor(percentagePedestriansCrossing) 244 | for i in range(0, len(all_id), 2): 245 | # start walker 246 | all_actors[i].start() 247 | # set walk to random point 248 | all_actors[i].go_to_location(world.get_random_location_from_navigation()) 249 | # max speed 250 | all_actors[i].set_max_speed(float(walker_speed[int(i/2)])) 251 | 252 | print('spawned %d vehicles and %d walkers, press Ctrl+C to exit.' % (len(vehicles_list), len(walkers_list))) 253 | 254 | # Example of how to use Traffic Manager parameters 255 | traffic_manager.global_percentage_speed_difference(30.0) 256 | 257 | 258 | # -------------- 259 | # Spawn attached RGB camera 260 | # -------------- 261 | 262 | car_locations = [] 263 | 264 | data = { 265 | 'image_name': [], 266 | 'steer':[], 267 | 'throttle':[], 268 | 'brake':[], 269 | 'speed':[], 270 | 'x':[], 271 | 'y':[], 272 | 'z':[], 273 | 'yaw':[], 274 | 'speed_limit':[], 275 | 'is_traffic_light':[], 276 | 'traffic_light_state':[], 277 | 'GPS_index':[], 278 | } 279 | 280 | 281 | def convert_vector_to_scalar(carlavect): 282 | # print(carlavect.length(), np.sqrt(carlavect.dot(carlavect))) 283 | # assert carlavect.length() == np.sqrt(carlavect.dot(carlavect)) 284 | return carlavect.length() 285 | 286 | 287 | def data_handler(image): 288 | img_name = dt_string + '_' + str(image.frame) + '.jpg' 289 | image.save_to_disk(os.path.join(image_dir, img_name)) 290 | 291 | control = ego_vehicle.get_control() 292 | transform = ego_vehicle.get_transform() 293 | velocity = ego_vehicle.get_velocity().length() 294 | speed_limit = ego_vehicle.get_speed_limit() 295 | light_state = ego_vehicle.get_traffic_light_state() 296 | is_traffic_light = ego_vehicle.is_at_traffic_light() 297 | traffic_light = ego_vehicle.get_traffic_light() 298 | data['image_name'].append(img_name) 299 | data['steer'].append(control.steer) 300 | data['throttle'].append(control.throttle) 301 | data['brake'].append(control.brake) 302 | data['speed'].append(velocity) 303 | data['x'].append(transform.location.x) 304 | data['y'].append(transform.location.y) 305 | data['z'].append(transform.location.z) 306 | data['yaw'].append(transform.rotation.yaw) 307 | data['speed_limit'].append(speed_limit) 308 | data['is_traffic_light'].append(is_traffic_light) 309 | data['traffic_light_state'].append(light_state) 310 | 311 | car_locations.append(ego_vehicle.get_location()) 312 | 313 | 314 | 315 | 316 | cam_bp = None 317 | cam_bp = world.get_blueprint_library().find('sensor.camera.rgb') 318 | cam_bp.set_attribute("image_size_x",str(IMAGE_HEIGHT)) 319 | cam_bp.set_attribute("image_size_y",str(IMAGE_WIDTH)) 320 | cam_bp.set_attribute("fov",str(105)) 321 | cam_location = carla.Location(2,0,1) 322 | cam_rotation = carla.Rotation(0,0,0) 323 | cam_transform = carla.Transform(cam_location,cam_rotation) 324 | ego_cam = world.spawn_actor(cam_bp,cam_transform,attach_to=ego_vehicle, attachment_type=carla.AttachmentType.Rigid) 325 | 326 | 327 | ego_vehicle.set_autopilot(True) 328 | print('\nEgo autopilot enabled') 329 | 330 | ego_cam.listen(data_handler) 331 | world_snapshot = world.wait_for_tick() 332 | 333 | try: 334 | i = 1 335 | while i <= RECORD_LENGTH: 336 | world_snapshot = world.wait_for_tick() 337 | print(f"Frame {str(i)} saved") 338 | i += 1 339 | 340 | except Exception as inst: 341 | print('\nSimulation error:') 342 | print(inst) 343 | 344 | 345 | df = pd.DataFrame(data=data) 346 | df.to_csv(os.path.join(rec_dir, 'data.csv'), index=False) 347 | 348 | 349 | if ego_vehicle is not None: 350 | if ego_cam is not None: 351 | ego_cam.stop() 352 | ego_cam.destroy() 353 | ego_vehicle.destroy() 354 | 355 | time.sleep(0.5) 356 | 357 | print('\ndestroying %d vehicles' % len(vehicles_list)) 358 | client.apply_batch([carla.command.DestroyActor(x) for x in vehicles_list]) 359 | 360 | # stop walker controllers (list is [controller, actor, controller, actor ...]) 361 | for i in range(0, len(all_id), 2): 362 | all_actors[i].stop() 363 | 364 | print('\ndestroying %d walkers' % len(walkers_list)) 365 | client.apply_batch([carla.command.DestroyActor(x) for x in all_id]) 366 | 367 | time.sleep(0.5) 368 | 369 | print("\nSimulation finished") 370 | 371 | print("\nSetting Waypoints' Coordinates") 372 | map = world.get_map() 373 | sampling_resolution = 2 374 | grp = GlobalRoutePlanner(map, sampling_resolution) 375 | 376 | def other_side_of_road(waypoint): 377 | """ 378 | Return the waypoint on the other side of the road. 379 | """ 380 | if waypoint.road_idx == 0: 381 | return waypoint.get_right_lane() 382 | else: 383 | return waypoint.get_left_lane() 384 | 385 | waypoints = [] 386 | waypoints_p = [] 387 | waypoints_m = [] 388 | 389 | for idx, loc in enumerate(car_locations): 390 | 391 | i = idx + 1 392 | if i >= len(car_locations): 393 | break 394 | while car_locations[i].distance(car_locations[idx]) < 100: 395 | i += 1 396 | 397 | w1 = grp.trace_route(car_locations[idx], car_locations[i]) 398 | 399 | wp = [] 400 | wp_p = [] 401 | wp_m = [] 402 | 403 | for w in w1: 404 | print(w) 405 | wp.append([w[0].transform.location.x, w[0].transform.location.y, w[0].transform.location.z]) 406 | 407 | w_p = other_side_of_road(w[0]) 408 | wp_p.append([w_p.transform.location.x, w_p.transform.location.y, w_p.transform.location.z]) 409 | 410 | x = (w[0].transform.location.x + w_p.transform.location.x)/2 411 | y = (w[0].transform.location.y + w_p.transform.location.y)/2 412 | z = (w[0].transform.location.z + w_p.transform.location.z)/2 413 | l_m = carla.Location(x, y, z) 414 | wp_m.append([x, y, z]) 415 | 416 | waypoints.append(wp) 417 | waypoints_p.append(wp_p) 418 | waypoints_m.append(wp_m) 419 | 420 | 421 | print("\nData retrieval finished") 422 | print(rec_dir) 423 | 424 | np.save(os.path.join(rec_dir, 'waypoints'), waypoints) 425 | np.save(os.path.join(rec_dir, 'waypoints_p'), waypoints_p) 426 | np.save(os.path.join(rec_dir, 'waypoints_m'), waypoints_m) 427 | 428 | -------------------------------------------------------------------------------- /Carla/retrive_data/misc.py: -------------------------------------------------------------------------------- 1 | import os 2 | from datetime import datetime 3 | 4 | def create_dir(path): 5 | # datetime object containing current date and time 6 | now = datetime.now() 7 | # dd/mm/YY H:M:S 8 | dt_string = now.strftime("%Y-%m-%d_%H-%M-%S") 9 | print('\nStarted at =', dt_string) 10 | 11 | rec_dir = os.path.join(path, dt_string) 12 | image_dir = os.path.join(rec_dir, 'images') 13 | try: 14 | os.makedirs(image_dir) 15 | print("\n"+ rec_dir + ' created') 16 | except Exception as inst: 17 | print("\nError: ") 18 | print(inst) -------------------------------------------------------------------------------- /Carla/waypoints/1.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TroddenSpade/Flexible-Conditional-Imitation-Learning/7ce40ffeac69f3ed09afa3523ef32adb91ad5355/Carla/waypoints/1.npy -------------------------------------------------------------------------------- /Carla/waypoints/2.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TroddenSpade/Flexible-Conditional-Imitation-Learning/7ce40ffeac69f3ed09afa3523ef32adb91ad5355/Carla/waypoints/2.npy -------------------------------------------------------------------------------- /Carla/waypoints/pos: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TroddenSpade/Flexible-Conditional-Imitation-Learning/7ce40ffeac69f3ed09afa3523ef32adb91ad5355/Carla/waypoints/pos -------------------------------------------------------------------------------- /Carla/waypoints/pos.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TroddenSpade/Flexible-Conditional-Imitation-Learning/7ce40ffeac69f3ed09afa3523ef32adb91ad5355/Carla/waypoints/pos.npy -------------------------------------------------------------------------------- /Carla/waypoints/w1: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TroddenSpade/Flexible-Conditional-Imitation-Learning/7ce40ffeac69f3ed09afa3523ef32adb91ad5355/Carla/waypoints/w1 -------------------------------------------------------------------------------- /Carla/waypoints/wayppp.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TroddenSpade/Flexible-Conditional-Imitation-Learning/7ce40ffeac69f3ed09afa3523ef32adb91ad5355/Carla/waypoints/wayppp.npy -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Flexible Conditional Imitation Learning 2 | The purpose of this project is to implement the Conditional Imitation Learning on various scenarios with `Airsim Car` plugin and `Carla` Environments. The original Conditional Imitation Learning propose a method in which a single navigatorial command is employed to take a certain action in perplexing situations. Instead, to increase the flexibility, this project concentrates on conditions similar to a path--a set of points on a plane--whereby we could present any kind of turns and velocities. 3 | 4 | 5 | ## Requirements 6 | * Tensorflow 7 | * Unreal Engine 4 (UE4) 8 | * [Airsim](https://microsoft.github.io/AirSim/) 9 | * [Carla](http://carla.org) 10 | 11 | 12 | ## Experiments 13 | Three different strategies are assessed. 14 | 15 | ### Mountainous UE4 Environment 16 | 17 | 18 | In [this environment](https://www.unrealengine.com/marketplace/en-US/product/landscape-mountains), the goal is to create an autonumous agent to drive through a tirtuous and mountainous road with the help of `Behavioral Cloning` and `Dataset Aggregation` approaches. 19 | 20 | 21 | #### Behavorial Cloning 22 | [//]: <> (what is bc and how it was implemented) 23 | 24 | 25 | #### Dataset Aggregation (DAgger) 26 | [//]: <> (how we are going to benefit from dagger) 27 | 28 | | | 29 | | :--: | 30 | | Collecting Data for Dataset Aggregation | 31 | 32 | #### Demo 33 | 34 | | | 35 | | :--: | 36 | | Autonomous Car Driving in Mountainous Environment | 37 | 38 | ### Neighborhood UE4 Env 39 | 40 | 41 | [This Env](https://www.unrealengine.com/marketplace/en-US/product/modular-neighborhood-pack) 42 | 43 | [//]: <> (talk about env) 44 | 45 | 46 | 47 | https://user-images.githubusercontent.com/33734646/212615848-410f06df-7442-4933-b292-018bbfc02c86.mp4 48 | 49 | 50 | 51 | https://user-images.githubusercontent.com/33734646/212620082-467928ee-6ff3-4421-8801-97a43983d04b.mp4 52 | 53 | 54 | 55 | #### Conditional Imitation Learning 56 | [//]: <> (how it was implemented and the differences with typical BC) 57 | 58 | 59 | ### Carla Town 60 | 61 | 62 | 63 | #### Flexible conditions 64 | 65 | 66 | 67 | #### Path Prediction 68 | 69 | ![Car](https://user-images.githubusercontent.com/33734646/211207480-aa3eb135-7d19-410c-bf5b-eb58a1881972.png) 70 | 71 | 72 | #### Translator 73 | 74 | 75 | ## References 76 | 77 | 1. Codevilla, F., Müller, M., López, A., Koltun, V., and Dosovitskiy, A., “End-to-end Driving via Conditional Imitation Learning”, arXiv e-prints, 2017. 78 | 2. Codevilla, F., Santana, E., Lopez, A., & Gaidon, A. (2019). Exploring the limitations of behavior cloning for autonomous driving. Proceedings of the IEEE International Conference on Computer Vision, 2019-Octob(Cvc), 9328–9337. https://doi.org/10.1109/ICCV.2019.00942 79 | 3. Rhinehart, N., McAllister, R., & Levine, S. (2018). Deep Imitative Models for Flexible Inference, Planning, and Control. 1, 1–19. http://arxiv.org/abs/1810.06544 80 | -------------------------------------------------------------------------------- /assets/dagger.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TroddenSpade/Flexible-Conditional-Imitation-Learning/7ce40ffeac69f3ed09afa3523ef32adb91ad5355/assets/dagger.gif -------------------------------------------------------------------------------- /assets/mountanous-auto-car.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TroddenSpade/Flexible-Conditional-Imitation-Learning/7ce40ffeac69f3ed09afa3523ef32adb91ad5355/assets/mountanous-auto-car.gif --------------------------------------------------------------------------------