├── .gitignore ├── LICENSE ├── README.md ├── imu_plot.py ├── map_plot.py ├── radar_plotter.py └── requirements.txt /.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 | *.egg-info/ 24 | .installed.cfg 25 | *.egg 26 | MANIFEST 27 | 28 | # PyInstaller 29 | # Usually these files are written by a python script from a template 30 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 31 | *.manifest 32 | *.spec 33 | 34 | # Installer logs 35 | pip-log.txt 36 | pip-delete-this-directory.txt 37 | 38 | # Unit test / coverage reports 39 | htmlcov/ 40 | .tox/ 41 | .coverage 42 | .coverage.* 43 | .cache 44 | nosetests.xml 45 | coverage.xml 46 | *.cover 47 | .hypothesis/ 48 | .pytest_cache/ 49 | 50 | # Translations 51 | *.mo 52 | *.pot 53 | 54 | # Django stuff: 55 | *.log 56 | local_settings.py 57 | db.sqlite3 58 | 59 | # Flask stuff: 60 | instance/ 61 | .webassets-cache 62 | 63 | # Scrapy stuff: 64 | .scrapy 65 | 66 | # Sphinx documentation 67 | docs/_build/ 68 | 69 | # PyBuilder 70 | target/ 71 | 72 | # Jupyter Notebook 73 | .ipynb_checkpoints 74 | 75 | # pyenv 76 | .python-version 77 | 78 | # celery beat schedule file 79 | celerybeat-schedule 80 | 81 | # SageMath parsed files 82 | *.sage.py 83 | 84 | # Environments 85 | .env 86 | .venv 87 | env/ 88 | venv/ 89 | ENV/ 90 | env.bak/ 91 | venv.bak/ 92 | 93 | # Spyder project settings 94 | .spyderproject 95 | .spyproject 96 | 97 | # Rope project settings 98 | .ropeproject 99 | 100 | # mkdocs documentation 101 | /site 102 | 103 | # mypy 104 | .mypy_cache/ 105 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Marc Garcia Puig 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Additional clients examples for Carla 2 | A set of Python client examples and utils for https://github.com/carla-simulator/carla. 3 | 4 | Most of them are intended for data validation and some of them are just simple examples. 5 | 6 | ## Usage: 7 | Once you have Carla up and running... 8 | 9 | ``` 10 | cd carla/PythonAPI/ 11 | git clone https://github.com/marcgpuig/carla_py_clients.git 12 | ``` 13 | -------------------------------------------------------------------------------- /imu_plot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2019 Marc G Puig. All rights reserved. 4 | # 5 | # This work is licensed under the terms of the MIT license. 6 | # For a copy, see . 7 | 8 | import glob 9 | import os 10 | import sys 11 | 12 | try: 13 | sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % ( 14 | sys.version_info.major, 15 | sys.version_info.minor, 16 | 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) 17 | except IndexError: 18 | pass 19 | 20 | import carla 21 | 22 | import matplotlib.pyplot as plt 23 | from matplotlib import animation 24 | 25 | import argparse 26 | import math 27 | import time 28 | import weakref 29 | 30 | 31 | def length(vec): 32 | return math.sqrt(vec.x * vec.x + vec.y * vec.y + vec.z * vec.z) 33 | 34 | def normalize_vector(vec): 35 | l = length(vec) 36 | if l: 37 | k = 1.0 / length(vec) 38 | vec.x *= k 39 | vec.y *= k 40 | vec.z *= k 41 | return vec 42 | return carla.Vector3D() 43 | 44 | def main(): 45 | argparser = argparse.ArgumentParser( 46 | description=__doc__) 47 | argparser.add_argument( 48 | '--host', 49 | metavar='H', 50 | default='127.0.0.1', 51 | help='IP of the host server (default: 127.0.0.1)') 52 | argparser.add_argument( 53 | '-p', '--port', 54 | metavar='P', 55 | default=2000, 56 | type=int, 57 | help='TCP port to listen to (default: 2000)') 58 | args = argparser.parse_args() 59 | 60 | # Time in seconds 61 | time_plot = [] 62 | # [[x], [y], [z]] values rad/s 63 | gyros_plot = [[], [], []] 64 | # [[x], [y], [z]] values m/s 65 | acc_plot = [[], [], []] 66 | 67 | try: 68 | 69 | client = carla.Client(args.host, args.port) 70 | client.set_timeout(2.0) 71 | world = client.get_world() 72 | debug = world.debug 73 | 74 | bp_lb = world.get_blueprint_library() 75 | imu_bp = bp_lb.filter('sensor.other.imu')[0] 76 | gnss_bp = bp_lb.filter('sensor.other.gnss')[0] 77 | mustang_bp = bp_lb.filter('vehicle.ford.mustang')[0] 78 | 79 | imu_bp.set_attribute('noise_seed', '10') 80 | 81 | imu_bp.set_attribute('noise_accel_stddev_x', '0.1') 82 | imu_bp.set_attribute('noise_accel_stddev_y', '0.1') 83 | imu_bp.set_attribute('noise_accel_stddev_z', '0.1') 84 | 85 | imu_bp.set_attribute('noise_lat_stddev', '0.1') 86 | 87 | imu_bp.set_attribute('noise_gyro_stddev_x', '0.1') 88 | imu_bp.set_attribute('noise_gyro_stddev_z', '0.1') 89 | imu_bp.set_attribute('noise_gyro_stddev_y', '0.1') 90 | 91 | start_location = carla.Transform(carla.Location(-47.7, -83.9, 5.5)) 92 | # start_location = world.get_map().get_spawn_points()[0].location 93 | 94 | print(world.get_map()) 95 | # print(world.get_map().get_spawn_points()) 96 | 97 | mustang = world.spawn_actor( 98 | mustang_bp, 99 | start_location) 100 | 101 | imu = world.spawn_actor( 102 | imu_bp, 103 | carla.Transform( 104 | location=carla.Location(y=1.5, z=1.5), 105 | rotation=carla.Rotation()), 106 | # rotation=carla.Rotation(pitch=45.0)), 107 | # rotation=carla.Rotation(pitch=90.0)), 108 | mustang) 109 | 110 | gnss = world.spawn_actor( 111 | gnss_bp, 112 | carla.Transform(), 113 | mustang) 114 | 115 | mustang.apply_control(carla.VehicleControl(throttle=0.3, steer=-0.0)) 116 | 117 | vec_size = 10.0 118 | 119 | init_time = world.get_snapshot().timestamp.elapsed_seconds 120 | 121 | def imu_callback(weak_imu, sensor): 122 | self = weak_imu() 123 | if not self: 124 | return 125 | # print(sensor) 126 | imu_tr = imu.get_transform() 127 | imu_pos = imu_tr.location 128 | fw_vec = imu_tr.get_forward_vector() 129 | 130 | normalized_gy = normalize_vector(sensor.gyroscope) 131 | 132 | # debug.draw_line( 133 | # imu_pos, 134 | # imu_pos + (normalized_gy * (vec_size * length(sensor.gyroscope))), 135 | # thickness=0.05, 136 | # color=carla.Color(255, 0, 0), 137 | # life_time=snapshot.timestamp.delta_seconds, 138 | # persistent_lines=False) 139 | 140 | # Plotting ####################################### 141 | curr_time = sensor.timestamp - init_time 142 | 143 | sys.stdout.write(f"\rElapsed time: {curr_time:4.1f}/{time_to_run} s") 144 | sys.stdout.flush() 145 | 146 | time_plot.append(curr_time) 147 | 148 | # gyros_plot[0].append(sensor.gyroscope.x) 149 | # total = 0.0 150 | # for i in gyros_plot[0]: 151 | # total += i 152 | # total /= len(gyros_plot[0]) 153 | # gyros_plot[1].append(total) 154 | gyros_plot[0].append(sensor.gyroscope.x) 155 | gyros_plot[1].append(sensor.gyroscope.y) 156 | gyros_plot[2].append(sensor.gyroscope.z) 157 | 158 | acc_plot[0].append(sensor.accelerometer.x) 159 | acc_plot[1].append(sensor.accelerometer.y) 160 | acc_plot[2].append(sensor.accelerometer.z) 161 | 162 | 163 | # debug.draw_line( 164 | # imu_pos, 165 | # imu_pos + (fw_vec * vec_size), 166 | # thickness=0.05, 167 | # color=carla.Color(255, 0, 0), 168 | # life_time=snapshot.timestamp.delta_seconds, 169 | # persistent_lines=False) 170 | 171 | # angular velocity ####################################### 172 | # nnormal = sensor.gyroscope / (2.0 * math.pi) 173 | # rotated_fw_vec_x = carla.Location(x=vec_size * nnormal.x) 174 | # imu_tr.transform(rotated_fw_vec_x) 175 | # debug.draw_line( 176 | # imu_pos, 177 | # rotated_fw_vec_x, 178 | # thickness=0.05, 179 | # color=carla.Color(255, 0, 0), 180 | # life_time=snapshot.timestamp.delta_seconds, 181 | # persistent_lines=False) 182 | 183 | # rotated_fw_vec_y = carla.Location(y=vec_size * nnormal.y) 184 | # imu_tr.transform(rotated_fw_vec_y) 185 | # debug.draw_line( 186 | # imu_pos, 187 | # rotated_fw_vec_y, 188 | # thickness=0.05, 189 | # color=carla.Color(0, 255, 0), 190 | # life_time=snapshot.timestamp.delta_seconds, 191 | # persistent_lines=False) 192 | 193 | # rotated_fw_vec_z = carla.Location(z=vec_size * nnormal.z) 194 | # imu_tr.transform(rotated_fw_vec_z) 195 | # debug.draw_line( 196 | # imu_pos, 197 | # rotated_fw_vec_z, 198 | # thickness=0.05, 199 | # color=carla.Color(0, 0, 255), 200 | # life_time=snapshot.timestamp.delta_seconds, 201 | # persistent_lines=False) 202 | 203 | # compass ####################################### 204 | # north_vec = imu_tr.get_forward_vector() 205 | # carla.Transform(rotation=carla.Rotation( 206 | # yaw=math.degrees(sensor.compass))).transform(north_vec) 207 | # north_vec.z = 0.0 208 | # north_vec = normalize_vector(north_vec) 209 | 210 | # debug.draw_line( 211 | # imu_pos, 212 | # imu_pos + (north_vec * vec_size), 213 | # thickness=0.05, 214 | # color=carla.Color(255, 165, 0), 215 | # life_time=snapshot.timestamp.delta_seconds, 216 | # persistent_lines=False) 217 | 218 | # accelerometer ####################################### 219 | # imu_tr = imu.get_transform() 220 | # sens_tr = sensor.transform 221 | 222 | # carla.Transform(rotation=carla.Rotation( 223 | # yaw=math.degrees(sensor.compass))).transform(north_vec) 224 | # north_vec = normalize_vector(north_vec) 225 | 226 | # debug.draw_line( 227 | # imu_pos, 228 | # imu_pos + (north_vec * vec_size), 229 | # thickness=0.05, 230 | # color=carla.Color(255, 165, 0), 231 | # life_time=snapshot.timestamp.delta_seconds, 232 | # persistent_lines=False) 233 | 234 | weak_imu = weakref.ref(imu) 235 | imu.listen(lambda sensor: imu_callback(weak_imu, sensor)) 236 | 237 | time_to_run = 20.0 # in seconds 238 | 239 | close_time = time.time() + time_to_run 240 | 241 | rot = 0.0 242 | 243 | while time.time() < close_time: 244 | # while time.time() > 0: 245 | snapshot = world.wait_for_tick(2.0) 246 | # imu.set_transform(carla.Transform( 247 | # carla.Location(), 248 | # carla.Rotation(yaw=rot))) 249 | # rot += 5 250 | # imu_pos = mustang.get_location() 251 | # debug.draw_line( 252 | # imu_pos, 253 | # imu_pos + carla.Location(z=10), 254 | # life_time=snapshot.timestamp.delta_seconds, 255 | # persistent_lines=False) 256 | # print(f'frame: {snapshot.frame}') 257 | # print(f'timestamp: {snapshot.timestamp}') 258 | 259 | 260 | finally: 261 | print('') 262 | imu.destroy() 263 | mustang.destroy() 264 | 265 | # plt.xkcd() # lol 266 | 267 | ############################################ 268 | 269 | # plt.title("Simple Plot") 270 | # plt.figure() 271 | 272 | # plt.subplot(211, title='gyroscope') # ------------------------------------------ 273 | 274 | # plt.tick_params(labelbottom=False) 275 | # plt.ylabel('angular velocity (rad)') 276 | 277 | # plt.grid(True) 278 | 279 | # plt.plot(time_plot[2:], gyros_plot[0][2:], color='red', label='X') 280 | # plt.plot(time_plot[2:], gyros_plot[1][2:], color='green', label='Y') 281 | # plt.plot(time_plot[2:], gyros_plot[2][2:], color='blue', label='Z') 282 | 283 | # plt.legend() 284 | 285 | # plt.subplot(212, title='accelerometer') # ------------------------------------------ 286 | 287 | # plt.ylabel('accelerometer (m/s)') 288 | # plt.xlabel('time (s)') 289 | 290 | # plt.grid(True) 291 | 292 | # plt.plot(time_plot[2:], acc_plot[0][2:], color='red', label='X') 293 | # plt.plot(time_plot[2:], acc_plot[1][2:], color='green', label='Y') 294 | # plt.plot(time_plot[2:], acc_plot[2][2:], color='blue', label='Z') 295 | 296 | # plt.legend() 297 | 298 | # plt.show() 299 | 300 | ############################################ 301 | 302 | # plt.subplot(131) 303 | # plt.hist(acc_plot[0], bins=50) 304 | # plt.title('X') 305 | 306 | # plt.subplot(132) 307 | # plt.hist(acc_plot[1], bins=50) 308 | # plt.title('Y') 309 | 310 | # plt.subplot(133) 311 | # plt.hist(acc_plot[2], bins=50) 312 | # plt.title('Z') 313 | 314 | # plt.show() 315 | 316 | ############################################ 317 | 318 | # Gyroscope Figure 319 | fig_g, ax_g = plt.subplots(figsize=(10, 4)) 320 | ax_g.set(xlim=(0, 20), ylim=(-5, 5)) 321 | ax_g.set_xlabel('Time (s)') 322 | ax_g.set_ylabel('Gyroscope (rad/s)') 323 | ax_g.grid(True) 324 | 325 | plot_gyros_x, = plt.plot([], [], color='red', label='X') 326 | plot_gyros_y, = plt.plot([], [], color='green', label='Y') 327 | plot_gyros_z, = plt.plot([], [], color='blue', label='Z') 328 | 329 | ax_g.legend() 330 | 331 | def gyros_init(): 332 | plot_gyros_x.set_data(time_plot[2], gyros_plot[0][2]) 333 | plot_gyros_y.set_data(time_plot[2], gyros_plot[1][2]) 334 | plot_gyros_z.set_data(time_plot[2], gyros_plot[2][2]) 335 | return plot_gyros_x, plot_gyros_y, plot_gyros_z, 336 | 337 | def gyros_animate(i): 338 | ax_g.set_title(f"Frame {i}") 339 | ax_g.set(xlim=(time_plot[i]-5.0, time_plot[i])) 340 | 341 | plot_gyros_x.set_data(time_plot[:i], gyros_plot[0][:i]) 342 | plot_gyros_y.set_data(time_plot[:i], gyros_plot[1][:i]) 343 | plot_gyros_z.set_data(time_plot[:i], gyros_plot[2][:i]) 344 | return plot_gyros_x, plot_gyros_y, plot_gyros_z, 345 | 346 | gyros_anim = animation.FuncAnimation( 347 | fig_g, 348 | gyros_animate, 349 | init_func=gyros_init, 350 | frames=len(time_plot), 351 | interval=16.6, 352 | blit=True, 353 | repeat=True) 354 | 355 | # Acceleration Figure 356 | fig_a, ax_a = plt.subplots(figsize=(10, 4)) 357 | ax_a.set(xlim=(0, 20), ylim=(-15, 20)) 358 | ax_a.set_xlabel('Time (s)') 359 | ax_a.set_ylabel('Accelerometer (m/s^2)') 360 | ax_a.grid(True) 361 | 362 | plot_acc_x, = ax_a.plot([], [], color='red', label='X') 363 | plot_acc_y, = ax_a.plot([], [], color='green', label='Y') 364 | plot_acc_z, = ax_a.plot([], [], color='blue', label='Z') 365 | 366 | ax_a.legend() 367 | 368 | def acc_init(): 369 | plot_acc_x.set_data(time_plot[2], acc_plot[0][2]) 370 | plot_acc_y.set_data(time_plot[2], acc_plot[1][2]) 371 | plot_acc_z.set_data(time_plot[2], acc_plot[2][2]) 372 | return plot_acc_x, plot_acc_y, plot_acc_z, 373 | 374 | def acc_animate(i): 375 | ax_a.set_title(f"Frame {i}") 376 | ax_a.set(xlim=(time_plot[i]-5.0, time_plot[i])) 377 | 378 | plot_acc_x.set_data(time_plot[:i], acc_plot[0][:i]) 379 | plot_acc_y.set_data(time_plot[:i], acc_plot[1][:i]) 380 | plot_acc_z.set_data(time_plot[:i], acc_plot[2][:i]) 381 | return plot_acc_x, plot_acc_y, plot_acc_z, 382 | 383 | acc_anim = animation.FuncAnimation( 384 | fig_a, 385 | acc_animate, 386 | init_func=acc_init, 387 | frames=len(time_plot), 388 | interval=16.6, 389 | blit=True, 390 | repeat=True) 391 | 392 | # gyros_anim.save('../../../Videos/gyros_plot.mp4', fps=60, extra_args=['-vcodec', 'libx264']) 393 | # acc_anim.save('../../../Videos/acc_plot.mp4', fps=60, extra_args=['-vcodec', 'libx264']) 394 | 395 | plt.show() 396 | 397 | 398 | if __name__ == '__main__': 399 | 400 | main() 401 | -------------------------------------------------------------------------------- /map_plot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2019 Marc G Puig. All rights reserved. 4 | # 5 | # This work is licensed under the terms of the MIT license. 6 | # For a copy, see . 7 | 8 | import glob 9 | import os 10 | import sys 11 | 12 | try: 13 | sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % ( 14 | sys.version_info.major, 15 | sys.version_info.minor, 16 | 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) 17 | except IndexError: 18 | pass 19 | 20 | import carla 21 | 22 | import argparse 23 | 24 | def main(): 25 | argparser = argparse.ArgumentParser() 26 | argparser.add_argument( 27 | '--host', 28 | metavar='H', 29 | default='127.0.0.1', 30 | help='IP of the host server (default: 127.0.0.1)') 31 | argparser.add_argument( 32 | '-p', '--port', 33 | metavar='P', 34 | default=2000, 35 | type=int, 36 | help='TCP port to listen to (default: 2000)') 37 | args = argparser.parse_args() 38 | 39 | # Approximate distance between the waypoints 40 | WAYPOINT_DISTANCE = 5.0 # in meters 41 | 42 | try: 43 | client = carla.Client(args.host, args.port) 44 | client.set_timeout(2.0) 45 | 46 | world = client.get_world() 47 | carla_map = world.get_map() 48 | 49 | import matplotlib.pyplot as plt 50 | 51 | plt.subplot(211) 52 | # Invert the y axis since we follow UE4 coordinates 53 | plt.gca().invert_yaxis() 54 | plt.margins(x=0.7, y=0) 55 | 56 | # GET WAYPOINTS IN THE MAP ########################################## 57 | # Returns a list of waypoints positioned on the center of the lanes 58 | # all over the map with an approximate distance between them. 59 | waypoint_list = carla_map.generate_waypoints(WAYPOINT_DISTANCE) 60 | 61 | plt.plot( 62 | [wp.transform.location.x for wp in waypoint_list], 63 | [wp.transform.location.y for wp in waypoint_list], 64 | linestyle='', markersize=3, color='blue', marker='o') 65 | ##################################################################### 66 | 67 | plt.subplot(212) 68 | # Invert the y axis since we follow UE4 coordinates 69 | plt.gca().invert_yaxis() 70 | plt.margins(x=0.7, y=0) 71 | 72 | # GET WAYPOINTS IN THE MAP ########################################## 73 | # It provides a minimal graph of the topology of the current OpenDRIVE file. 74 | # It is constituted by a list of pairs of waypoints, where the first waypoint 75 | # is the origin and the second one is the destination. 76 | # It can be loaded into NetworkX. 77 | # A valid output could be: [ (w0, w1), (w0, w2), (w1, w3), (w2, w3), (w0, w4) ] 78 | topology = carla_map.get_topology() 79 | road_list = [] 80 | 81 | for wp_pair in topology: 82 | current_wp = wp_pair[0] 83 | # Check if there is a road with no previus road, this can happen 84 | # in opendrive. Then just continue. 85 | if current_wp is None: 86 | continue 87 | # First waypoint on the road that goes from wp_pair[0] to wp_pair[1]. 88 | current_road_id = current_wp.road_id 89 | wps_in_single_road = [current_wp] 90 | # While current_wp has the same road_id (has not arrived to next road). 91 | while current_wp.road_id == current_road_id: 92 | # Check for next waypoints in aprox distance. 93 | available_next_wps = current_wp.next(WAYPOINT_DISTANCE) 94 | # If there is next waypoint/s? 95 | if available_next_wps: 96 | # We must take the first ([0]) element because next(dist) can 97 | # return multiple waypoints in intersections. 98 | current_wp = available_next_wps[0] 99 | wps_in_single_road.append(current_wp) 100 | else: # If there is no more waypoints we can stop searching for more. 101 | break 102 | road_list.append(wps_in_single_road) 103 | 104 | # Plot each road (on a different color by default) 105 | for road in road_list: 106 | plt.plot( 107 | [wp.transform.location.x for wp in road], 108 | [wp.transform.location.y for wp in road]) 109 | ##################################################################### 110 | 111 | plt.show() 112 | 113 | finally: 114 | pass 115 | 116 | 117 | if __name__ == '__main__': 118 | try: 119 | main() 120 | finally: 121 | print('Done.') 122 | -------------------------------------------------------------------------------- /radar_plotter.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2019 Marc G Puig. All rights reserved. 4 | # 5 | # This work is licensed under the terms of the MIT license. 6 | # For a copy, see . 7 | 8 | import glob 9 | import os 10 | import sys 11 | 12 | try: 13 | sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % ( 14 | sys.version_info.major, 15 | sys.version_info.minor, 16 | 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) 17 | except IndexError: 18 | pass 19 | 20 | import carla 21 | 22 | import argparse 23 | import math 24 | import time 25 | import weakref 26 | 27 | 28 | def get_correct_type(attr): 29 | if attr.type == carla.ActorAttributeType.Bool: 30 | return attr.as_bool() 31 | if attr.type == carla.ActorAttributeType.Int: 32 | return attr.as_int() 33 | if attr.type == carla.ActorAttributeType.Float: 34 | return attr.as_float() 35 | if attr.type == carla.ActorAttributeType.String: 36 | return attr.as_str() 37 | if attr.type == carla.ActorAttributeType.RGBColor: 38 | return attr.as_color() 39 | 40 | 41 | def main(): 42 | argparser = argparse.ArgumentParser( 43 | description=__doc__) 44 | argparser.add_argument( 45 | '--host', 46 | metavar='H', 47 | default='127.0.0.1', 48 | help='IP of the host server (default: 127.0.0.1)') 49 | argparser.add_argument( 50 | '-p', '--port', 51 | metavar='P', 52 | default=2000, 53 | type=int, 54 | help='TCP port to listen to (default: 2000)') 55 | argparser.add_argument( 56 | '--fps', 57 | metavar='N', 58 | default=30, 59 | type=int, 60 | help='set fixed FPS, zero for variable FPS') 61 | argparser.add_argument( 62 | '--vertical-fov', 63 | metavar='N', 64 | default=30.0, 65 | type=float, 66 | help='radar\'s vertical FOV (default: 30.0)') 67 | argparser.add_argument( 68 | '--horizontal-fov', 69 | metavar='N', 70 | default=30.0, 71 | type=float, 72 | help='radar\'s horizontal FOV (default: 30.0)') 73 | argparser.add_argument( 74 | '--points-per-second', 75 | metavar='N', 76 | default=1500, 77 | type=int, 78 | help='radar\'s total points per second (default: 1500)') 79 | argparser.add_argument( 80 | '--range', 81 | metavar='D', 82 | default=100.0, 83 | type=float, 84 | help='radar\'s maximum range in meters (default: 100.0)') 85 | argparser.add_argument( 86 | '--time', 87 | metavar='S', 88 | default=10, 89 | type=float, 90 | help='time to run in seconds (default: 10)') 91 | args = argparser.parse_args() 92 | 93 | try: 94 | 95 | client = carla.Client(args.host, args.port) 96 | client.set_timeout(2.0) 97 | world = client.get_world() 98 | 99 | settings = world.get_settings() 100 | settings.synchronous_mode = True 101 | settings.fixed_delta_seconds = (1.0 / args.fps) if args.fps > 0.0 else 0.0 102 | world.apply_settings(settings) 103 | 104 | bp_lb = world.get_blueprint_library() 105 | radar_bp = bp_lb.filter('sensor.other.radar')[0] 106 | 107 | # Set Radar attributes, by default are: 108 | radar_bp.set_attribute('horizontal_fov', str(args.horizontal_fov)) # degrees 109 | radar_bp.set_attribute('vertical_fov', str(args.vertical_fov)) # degrees 110 | radar_bp.set_attribute('points_per_second', str(args.points_per_second)) 111 | radar_bp.set_attribute('range', str(args.range)) # meters 112 | 113 | # Display Radar attributes 114 | print(f"{'- Radar Information ':-<45}") 115 | print(f" - {'ID':<23}{radar_bp.id}") 116 | for attr in radar_bp: 117 | print(f" - {attr.id+':':<23}{get_correct_type(attr)}") 118 | print(f"{'':-<45}") 119 | 120 | mustang_bp = bp_lb.filter('vehicle.ford.mustang')[0] 121 | 122 | start_location = world.get_map().get_spawn_points()[0] 123 | mustang = world.spawn_actor( 124 | mustang_bp, 125 | start_location) 126 | 127 | mustang.set_autopilot(True) 128 | 129 | radar = world.spawn_actor( 130 | radar_bp, 131 | carla.Transform(location=carla.Location(y=1.5, z=0.5)), 132 | mustang) 133 | 134 | camera_bp = bp_lb.filter('sensor.camera.rgb')[0] 135 | image_size = (600, 400) 136 | camera_bp.set_attribute('image_size_x', str(image_size[0])) 137 | camera_bp.set_attribute('image_size_y', str(image_size[1])) 138 | camera = world.spawn_actor( 139 | camera_bp, 140 | carla.Transform(location=carla.Location(y=1.5, z=0.5)), 141 | mustang) 142 | 143 | # Ploting variables 144 | p_depth = [] 145 | p_azimuth = [] 146 | p_altitude = [] 147 | p_velocity = [] 148 | p_images = [] 149 | 150 | def radar_callback(weak_radar, sensor): 151 | self = weak_radar() 152 | if not self: 153 | return 154 | 155 | current_depth = [] 156 | current_azimuth = [] 157 | current_altitude = [] 158 | current_velocity = [] 159 | 160 | for i in sensor: 161 | current_depth.append(i.depth) 162 | current_azimuth.append(i.azimuth) 163 | current_altitude.append(i.altitude) 164 | current_velocity.append(i.velocity) 165 | 166 | p_depth.append(current_depth) 167 | p_azimuth.append(current_azimuth) 168 | p_altitude.append(current_altitude) 169 | p_velocity.append(current_velocity) 170 | 171 | weak_radar = weakref.ref(radar) 172 | radar.listen(lambda sensor: radar_callback(weak_radar, sensor)) 173 | 174 | def camera_callback(weak_camera, image): 175 | self = weak_camera() 176 | if not self: 177 | return 178 | # p_images.append(image) 179 | image.save_to_disk('_out/%08d' % image.frame) 180 | 181 | weak_camera = weakref.ref(camera) 182 | camera.listen(lambda image: camera_callback(weak_camera, image)) 183 | 184 | time_to_run = args.time # in seconds 185 | initial_time = time.time() 186 | close_time = initial_time + time_to_run 187 | 188 | while time.time() < close_time: 189 | sys.stdout.write(f"\rElapsed time: {time.time() - initial_time:4.1f}/{time_to_run} s") 190 | sys.stdout.flush() 191 | world.tick() 192 | 193 | finally: 194 | print('') 195 | try: 196 | settings.synchronous_mode = False 197 | settings.fixed_delta_seconds = None 198 | world.apply_settings(settings) 199 | except NameError: 200 | pass 201 | 202 | try: 203 | radar.destroy() 204 | except NameError: 205 | pass 206 | 207 | try: 208 | camera.destroy() 209 | except NameError: 210 | pass 211 | 212 | try: 213 | mustang.destroy() 214 | except NameError: 215 | pass 216 | 217 | import numpy as np 218 | import matplotlib.pyplot as plt 219 | from matplotlib import animation 220 | 221 | p_depth = np.array([np.array(i) for i in p_depth]) 222 | p_azimuth = np.array([np.array(i) for i in p_azimuth]) 223 | p_altitude = np.array([np.array(i) for i in p_altitude]) 224 | p_velocity = np.array([np.array(i) for i in p_velocity]) 225 | p_images = np.array([np.array(i.raw_data) for i in p_images]) 226 | 227 | # im = np.frombuffer(p_images[5], dtype=np.dtype("uint8")) 228 | # im = im.reshape((image_size[1], image_size[0], 4)) 229 | # im = im[:, :, :3] 230 | # im = im[:, :, ::-1] 231 | 232 | # plt.imshow(im) 233 | # plt.show() 234 | 235 | # sys.exit(0) 236 | 237 | # Animation ################################################## 238 | fig, (ax_depth, ax_vel) = plt.subplots(nrows=1, ncols=2, figsize=(14, 6)) 239 | 240 | half_hor_fov_to_deg = math.radians(args.horizontal_fov) / 2.0 241 | half_ver_fov_to_deg = math.radians(args.vertical_fov) / 2.0 242 | horizontal_axes_limits = (-half_hor_fov_to_deg, half_hor_fov_to_deg) 243 | vertical_axes_limits = (-half_ver_fov_to_deg, half_ver_fov_to_deg) 244 | 245 | # - Depth Axes ---------------------------------- 246 | ax_depth.set(xlim=horizontal_axes_limits, ylim=vertical_axes_limits) 247 | ax_depth.set_title('Depth (m)') 248 | ax_depth.set_xlabel('Azimuth (rad)') 249 | ax_depth.set_ylabel('Altitude (rad)') 250 | 251 | # Initialize the depth scater plot without data 252 | scat_depth = ax_depth.scatter( 253 | np.array([]), 254 | np.array([]), 255 | c=np.array([]), # dot intensity/color 256 | s=20, # dot size 257 | vmin=0.0, # colorize from 0 m 258 | vmax=args.range, # to Radar's maximum disance (far) 259 | cmap='viridis') # viridis, plasma, gray... 260 | 261 | fig.colorbar(scat_depth, ax=ax_depth, extend='max', shrink=1.0, pad=0.01) 262 | 263 | # - Velocity Axes ------------------------------- 264 | ax_vel.set(xlim=horizontal_axes_limits, ylim=vertical_axes_limits) 265 | ax_vel.set_title('Velocity (m/s)') 266 | ax_vel.set_xlabel('Azimuth (rad)') 267 | ax_vel.set_ylabel('Altitude (rad)') 268 | 269 | # Initialize the velocity scater plot without data 270 | scat_vel = ax_vel.scatter( 271 | np.array([]), 272 | np.array([]), 273 | c=np.array([]), # dot intensity/color 274 | s=20, # dot size 275 | vmin=-10.0, # colorize from -10 m/s 276 | vmax=10.0, # to +10 m/s 277 | cmap='RdBu') # RdBu, Spectral, coolwarm... 278 | 279 | fig.colorbar(scat_vel, ax=ax_vel, extend='both', shrink=1.0, pad=0.01) 280 | 281 | # Animation initialization callback 282 | def acc_init(): 283 | scat_depth.set_array(p_depth[0]) 284 | scat_vel.set_array(p_velocity[0]) 285 | 286 | scat_depth.set_offsets(np.c_[p_azimuth[0], p_altitude[0]]) 287 | scat_vel.set_offsets(np.c_[p_azimuth[0], p_altitude[0]]) 288 | 289 | return scat_depth, scat_vel 290 | 291 | # Animation update callback 292 | def radar_anim_update(i): 293 | scat_depth.set_array(p_depth[i]) 294 | scat_vel.set_array(p_velocity[i]) 295 | 296 | scat_depth.set_offsets(np.c_[p_azimuth[i], p_altitude[i]]) 297 | scat_vel.set_offsets(np.c_[p_azimuth[i], p_altitude[i]]) 298 | 299 | return scat_depth, scat_vel 300 | 301 | # Do the animation 302 | radar_animation = animation.FuncAnimation( 303 | fig, 304 | radar_anim_update, 305 | init_func=acc_init, 306 | frames=len(p_azimuth), 307 | interval=(1.0 / args.fps) if args.fps > 0.0 else 0.0, 308 | blit=True, 309 | repeat=True) 310 | 311 | # Store the animation as mp4 312 | radar_animation.save( 313 | f"./radar_plot{time.time()}.mp4", 314 | fps=args.fps, 315 | extra_args=["-vcodec", "libx264"]) 316 | ############################################################## 317 | 318 | if __name__ == '__main__': 319 | main() 320 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | numpy 2 | matplotlib 3 | 4 | --------------------------------------------------------------------------------