From f628309f8ce5d6af3cb86c8393b3c876e36a18ef Mon Sep 17 00:00:00 2001 From: nongfugengxia Date: Wed, 12 Jun 2024 17:12:48 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E4=BC=A0=E6=84=9F=E5=99=A8?= =?UTF-8?q?=E7=A4=BA=E4=BE=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- docs/ref_sensors.md | 16 +- src/examples/manual_control.py | 1 + src/examples/rgb_camera_example.py | 10 +- src/sensor/imu_plot.py | 399 +++++++++++++++++++++++++++++ src/sensor/map_plot.py | 122 +++++++++ src/sensor/radar_plotter.py | 321 +++++++++++++++++++++++ src/walker/pedestrian_eye.py | 251 ++++++++++++++++++ 7 files changed, 1107 insertions(+), 13 deletions(-) create mode 100644 src/sensor/imu_plot.py create mode 100644 src/sensor/map_plot.py create mode 100644 src/sensor/radar_plotter.py create mode 100644 src/walker/pedestrian_eye.py diff --git a/docs/ref_sensors.md b/docs/ref_sensors.md index cfa5c5c2d..3ceeec182 100644 --- a/docs/ref_sensors.md +++ b/docs/ref_sensors.md @@ -782,14 +782,14 @@ raw_image.save_to_disk("path/to/save/converted/image",carla.cityScapesPalette) --- -## DVS 相机 +## 动态视觉传感器相机 * __蓝图：__ sensor.camera.dvs * __输出：__ 每步 [carla.DVSEventArray](python_api.md#carla.DVSEventArray) （除非`sensor_tick`另有说明）。 -动态视觉传感器或事件相机是一种工作方式与传统相机完全不同的传感器。事件相机不是以固定速率捕获强度图像，而是以事件流的形式异步测量强度变化，对每个像素的亮度变化进行编码。与标准摄像机相比，事件摄像机具有独特的属性。它们具有非常高的动态范围（140 dB 与 60 dB）、无运动模糊和高时间分辨率（微秒级）。因此，事件相机是即使在具有挑战性的高速场景和高动态范围环境下也能提供高质量视觉信息的传感器，为基于视觉的算法提供了新的应用领域。 +[动态视觉传感器](https://baike.baidu.com/item/%E5%8A%A8%E6%80%81%E8%A7%86%E8%A7%89%E4%BC%A0%E6%84%9F%E5%99%A8/23490201) （Dynamic Vision Sensor, DVS）或事件相机是一种工作方式与传统相机完全不同的传感器。事件相机不是以固定速率捕获强度图像，而是以事件流的形式异步测量强度变化，对每个像素的亮度变化进行编码。与标准摄像机相比，事件摄像机具有独特的属性。它们具有非常高的动态范围（140 dB 与 60 dB）、无运动模糊和高时间分辨率（微秒级）。因此，事件相机是即使在具有挑战性的高速场景和高动态范围环境下也能提供高质量视觉信息的传感器，为基于视觉的算法提供了新的应用领域。 -DVS 摄像机输出事件流。当对数强度的变化达到预定义的恒定阈值（通常在 15% 到 30% 之间）时，在时间戳处的像素处`e=(x,y,t,pol)`触发事件。 +动态视觉传感器摄像机输出事件流。当对数强度的变化达到预定义的恒定阈值（通常在 15% 到 30% 之间）时，在时间戳处的像素处`e=(x,y,t,pol)`触发事件。 `` L(x,y,t) - L(x,y,t-\delta t) = pol C @@ -799,19 +799,19 @@ L(x,y,t) - L(x,y,t-\delta t) = pol C ![DVSCameraWorkingPrinciple](img/sensor_dvs_scheme.jpg) -DVS 摄像机的当前实现在两个连续同步帧之间以统一采样方式工作。因此，为了仿真真实事件相机的高时间分辨率（微秒级），传感器需要以高频率执行（比传统相机的频率高得多）。实际上，Carla 汽车行驶速度越快，事件数量就会增加。因此，传感器频率应随着场景的动态而相应增加。用户应该在时间精度和计算成本之间找到平衡。 +动态视觉传感器摄像机的当前实现在两个连续同步帧之间以统一采样方式工作。因此，为了仿真真实事件相机的高时间分辨率（微秒级），传感器需要以高频率执行（比传统相机的频率高得多）。实际上，Carla 汽车行驶速度越快，事件数量就会增加。因此，传感器频率应随着场景的动态而相应增加。用户应该在时间精度和计算成本之间找到平衡。 -提供的脚本[`manual_control.py`]使用 DVS 摄像头来展示如何配置传感器、如何获取事件流以及如何以图像格式（通常称为事件框架）描述此类事件。 +提供的脚本 [`manual_control.py`](https://github.com/OpenHUTB/carla_doc/blob/master/src/examples/manual_control.py) 使用动态视觉传感器摄像头来展示如何配置传感器、如何获取事件流以及如何以图像格式（通常称为事件框架）描述此类事件。 [manual_control]: https://github.com/carla-simulator/carla/blob/master/PythonAPI/examples/manual_control.py -请注意，由于 DVS 摄像机的采样方法，如果两个连续同步帧之间没有像素差异，摄像机将不会返回图像。这总是发生在第一帧中，因为没有前一帧可供比较，并且在帧之间没有移动的情况下也是如此。 +请注意，由于动态视觉传感器摄像机的采样方法，如果两个连续同步帧之间没有像素差异，摄像机将不会返回图像。这总是发生在第一帧中，因为没有前一帧可供比较，并且在帧之间没有移动的情况下也是如此。 ![DVSCameraWorkingPrinciple](img/sensor_dvs.gif) -DVS 是一个相机，因此具有 RGB 相机中可用的所有属性。然而，事件摄像机的工作原理几乎没有什么独有的属性。 +动态视觉传感器是一个相机，因此具有 RGB 相机中可用的所有属性。然而，事件摄像机的工作原理几乎没有什么独有的属性。 -#### DVS 相机属性 +#### 动态视觉传感器相机属性 | 蓝图属性 | 类型 | 默认 | 描述 | | ---------------------- | ---------------------- | ---------------------- | ---------------------- | diff --git a/src/examples/manual_control.py b/src/examples/manual_control.py index d095c5079..320fe6a13 100644 --- a/src/examples/manual_control.py +++ b/src/examples/manual_control.py @@ -1,4 +1,5 @@ #!/usr/bin/env python +# 参考：https://github.com/marcgpuig/carla_py_clients/blob/master/imu_plot.py # Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de # Barcelona (UAB). diff --git a/src/examples/rgb_camera_example.py b/src/examples/rgb_camera_example.py index cb42397e8..40c712c51 100644 --- a/src/examples/rgb_camera_example.py +++ b/src/examples/rgb_camera_example.py @@ -31,7 +31,7 @@ def initialize_world(args): - """Create world instance and create/retrieve hero actor""" + """创建世界实例并创建/获取英雄参与者""" client = carla.Client(args.host, args.port) carla_world = CarlaWorld(client) if args.map: @@ -182,10 +182,10 @@ def dvs_callback(data: carla.Image, data_dict: dict): ) sensor_data = setup_callbacks(camera_dict) - # OpenCV named window for display + # 在 OpenCV 窗口中显示的名字 cv2.namedWindow("All cameras", cv2.WINDOW_AUTOSIZE) - # Tile all data in one array + # 在一个数组中平铺所有的数据 top_row = np.concatenate( (sensor_data["rgb_image"], sensor_data["sem_image"], sensor_data["inst_image"]), axis=1, @@ -200,13 +200,13 @@ def dvs_callback(data: carla.Image, data_dict: dict): ) tiled = np.concatenate((top_row, lower_row), axis=0) - # Display with imshow + # 使用 imshow 函数显示 cv2.imshow("All cameras", tiled) cv2.waitKey(1) try: while True: - # Tile camera images into one array + # 将相机图像平铺进一个数组 top_row = np.concatenate( ( sensor_data["rgb_image"], diff --git a/src/sensor/imu_plot.py b/src/sensor/imu_plot.py new file mode 100644 index 000000000..f8ae3ae76 --- /dev/null +++ b/src/sensor/imu_plot.py @@ -0,0 +1,399 @@ +#!/usr/bin/env python + +# Copyright (c) 2019 Marc G Puig. All rights reserved. +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +import glob +import os +import sys + +try: + sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % ( + sys.version_info.major, + sys.version_info.minor, + 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) +except IndexError: + pass + +import carla + +import matplotlib.pyplot as plt +from matplotlib import animation + +import argparse +import math +import time +import weakref + + +def length(vec): + return math.sqrt(vec.x * vec.x + vec.y * vec.y + vec.z * vec.z) + +def normalize_vector(vec): + l = length(vec) + if l: + k = 1.0 / length(vec) + vec.x *= k + vec.y *= k + vec.z *= k + return vec + return carla.Vector3D() + +def main(): + argparser = argparse.ArgumentParser( + description=__doc__) + argparser.add_argument( + '--host', + metavar='H', + default='127.0.0.1', + help='IP of the host server (default: 127.0.0.1)') + argparser.add_argument( + '-p', '--port', + metavar='P', + default=2000, + type=int, + help='TCP port to listen to (default: 2000)') + args = argparser.parse_args() + + # Time in seconds + time_plot = [] + # [[x], [y], [z]] values rad/s + gyros_plot = [[], [], []] + # [[x], [y], [z]] values m/s + acc_plot = [[], [], []] + + try: + + client = carla.Client(args.host, args.port) + client.set_timeout(2.0) + world = client.get_world() + debug = world.debug + + bp_lb = world.get_blueprint_library() + imu_bp = bp_lb.filter('sensor.other.imu')[0] + gnss_bp = bp_lb.filter('sensor.other.gnss')[0] + mustang_bp = bp_lb.filter('vehicle.ford.mustang')[0] + + imu_bp.set_attribute('noise_seed', '10') + + imu_bp.set_attribute('noise_accel_stddev_x', '0.1') + imu_bp.set_attribute('noise_accel_stddev_y', '0.1') + imu_bp.set_attribute('noise_accel_stddev_z', '0.1') + + #imu_bp.set_attribute('noise_lat_stddev', '0.1') + + imu_bp.set_attribute('noise_gyro_stddev_x', '0.1') + imu_bp.set_attribute('noise_gyro_stddev_z', '0.1') + imu_bp.set_attribute('noise_gyro_stddev_y', '0.1') + + start_location = carla.Transform(carla.Location(-47.7, -83.9, 5.5)) + # start_location = world.get_map().get_spawn_points()[0].location + + print(world.get_map()) + # print(world.get_map().get_spawn_points()) + + mustang = world.spawn_actor( + mustang_bp, + start_location) + + imu = world.spawn_actor( + imu_bp, + carla.Transform( + location=carla.Location(y=1.5, z=1.5), + rotation=carla.Rotation()), + # rotation=carla.Rotation(pitch=45.0)), + # rotation=carla.Rotation(pitch=90.0)), + mustang) + + gnss = world.spawn_actor( + gnss_bp, + carla.Transform(), + mustang) + + mustang.apply_control(carla.VehicleControl(throttle=0.3, steer=-0.0)) + + vec_size = 10.0 + + init_time = world.get_snapshot().timestamp.elapsed_seconds + + def imu_callback(weak_imu, sensor): + self = weak_imu() + if not self: + return + # print(sensor) + imu_tr = imu.get_transform() + imu_pos = imu_tr.location + fw_vec = imu_tr.get_forward_vector() + + normalized_gy = normalize_vector(sensor.gyroscope) + + # debug.draw_line( + # imu_pos, + # imu_pos + (normalized_gy * (vec_size * length(sensor.gyroscope))), + # thickness=0.05, + # color=carla.Color(255, 0, 0), + # life_time=snapshot.timestamp.delta_seconds, + # persistent_lines=False) + + # Plotting ####################################### + curr_time = sensor.timestamp - init_time + + sys.stdout.write(f"\rElapsed time: {curr_time:4.1f}/{time_to_run} s") + sys.stdout.flush() + + time_plot.append(curr_time) + + # gyros_plot[0].append(sensor.gyroscope.x) + # total = 0.0 + # for i in gyros_plot[0]: + # total += i + # total /= len(gyros_plot[0]) + # gyros_plot[1].append(total) + gyros_plot[0].append(sensor.gyroscope.x) + gyros_plot[1].append(sensor.gyroscope.y) + gyros_plot[2].append(sensor.gyroscope.z) + + acc_plot[0].append(sensor.accelerometer.x) + acc_plot[1].append(sensor.accelerometer.y) + acc_plot[2].append(sensor.accelerometer.z) + + + # debug.draw_line( + # imu_pos, + # imu_pos + (fw_vec * vec_size), + # thickness=0.05, + # color=carla.Color(255, 0, 0), + # life_time=snapshot.timestamp.delta_seconds, + # persistent_lines=False) + + # angular velocity ####################################### + # nnormal = sensor.gyroscope / (2.0 * math.pi) + # rotated_fw_vec_x = carla.Location(x=vec_size * nnormal.x) + # imu_tr.transform(rotated_fw_vec_x) + # debug.draw_line( + # imu_pos, + # rotated_fw_vec_x, + # thickness=0.05, + # color=carla.Color(255, 0, 0), + # life_time=snapshot.timestamp.delta_seconds, + # persistent_lines=False) + + # rotated_fw_vec_y = carla.Location(y=vec_size * nnormal.y) + # imu_tr.transform(rotated_fw_vec_y) + # debug.draw_line( + # imu_pos, + # rotated_fw_vec_y, + # thickness=0.05, + # color=carla.Color(0, 255, 0), + # life_time=snapshot.timestamp.delta_seconds, + # persistent_lines=False) + + # rotated_fw_vec_z = carla.Location(z=vec_size * nnormal.z) + # imu_tr.transform(rotated_fw_vec_z) + # debug.draw_line( + # imu_pos, + # rotated_fw_vec_z, + # thickness=0.05, + # color=carla.Color(0, 0, 255), + # life_time=snapshot.timestamp.delta_seconds, + # persistent_lines=False) + + # compass ####################################### + # north_vec = imu_tr.get_forward_vector() + # carla.Transform(rotation=carla.Rotation( + # yaw=math.degrees(sensor.compass))).transform(north_vec) + # north_vec.z = 0.0 + # north_vec = normalize_vector(north_vec) + + # debug.draw_line( + # imu_pos, + # imu_pos + (north_vec * vec_size), + # thickness=0.05, + # color=carla.Color(255, 165, 0), + # life_time=snapshot.timestamp.delta_seconds, + # persistent_lines=False) + + # accelerometer ####################################### + # imu_tr = imu.get_transform() + # sens_tr = sensor.transform + + # carla.Transform(rotation=carla.Rotation( + # yaw=math.degrees(sensor.compass))).transform(north_vec) + # north_vec = normalize_vector(north_vec) + + # debug.draw_line( + # imu_pos, + # imu_pos + (north_vec * vec_size), + # thickness=0.05, + # color=carla.Color(255, 165, 0), + # life_time=snapshot.timestamp.delta_seconds, + # persistent_lines=False) + + weak_imu = weakref.ref(imu) + imu.listen(lambda sensor: imu_callback(weak_imu, sensor)) + + time_to_run = 20.0 # in seconds + + close_time = time.time() + time_to_run + + rot = 0.0 + + while time.time() < close_time: + # while time.time() > 0: + snapshot = world.wait_for_tick(2.0) + # imu.set_transform(carla.Transform( + # carla.Location(), + # carla.Rotation(yaw=rot))) + # rot += 5 + # imu_pos = mustang.get_location() + # debug.draw_line( + # imu_pos, + # imu_pos + carla.Location(z=10), + # life_time=snapshot.timestamp.delta_seconds, + # persistent_lines=False) + # print(f'frame: {snapshot.frame}') + # print(f'timestamp: {snapshot.timestamp}') + + finally: + print('') + #imu.destroy() + #mustang.destroy() + + # plt.xkcd() # lol + + ############################################ + + # plt.title("Simple Plot") + # plt.figure() + + # plt.subplot(211, title='gyroscope') # ------------------------------------------ + + # plt.tick_params(labelbottom=False) + # plt.ylabel('angular velocity (rad)') + + # plt.grid(True) + + # plt.plot(time_plot[2:], gyros_plot[0][2:], color='red', label='X') + # plt.plot(time_plot[2:], gyros_plot[1][2:], color='green', label='Y') + # plt.plot(time_plot[2:], gyros_plot[2][2:], color='blue', label='Z') + + # plt.legend() + + # plt.subplot(212, title='accelerometer') # ------------------------------------------ + + # plt.ylabel('accelerometer (m/s)') + # plt.xlabel('time (s)') + + # plt.grid(True) + + # plt.plot(time_plot[2:], acc_plot[0][2:], color='red', label='X') + # plt.plot(time_plot[2:], acc_plot[1][2:], color='green', label='Y') + # plt.plot(time_plot[2:], acc_plot[2][2:], color='blue', label='Z') + + # plt.legend() + + # plt.show() + + ############################################ + + # plt.subplot(131) + # plt.hist(acc_plot[0], bins=50) + # plt.title('X') + + # plt.subplot(132) + # plt.hist(acc_plot[1], bins=50) + # plt.title('Y') + + # plt.subplot(133) + # plt.hist(acc_plot[2], bins=50) + # plt.title('Z') + + # plt.show() + + ############################################ + + # Gyroscope Figure + fig_g, ax_g = plt.subplots(figsize=(10, 4)) + ax_g.set(xlim=(0, 20), ylim=(-5, 5)) + ax_g.set_xlabel('Time (s)') + ax_g.set_ylabel('Gyroscope (rad/s)') + ax_g.grid(True) + + plot_gyros_x, = plt.plot([], [], color='red', label='X') + plot_gyros_y, = plt.plot([], [], color='green', label='Y') + plot_gyros_z, = plt.plot([], [], color='blue', label='Z') + + ax_g.legend() + + def gyros_init(): + plot_gyros_x.set_data(time_plot[2], gyros_plot[0][2]) + plot_gyros_y.set_data(time_plot[2], gyros_plot[1][2]) + plot_gyros_z.set_data(time_plot[2], gyros_plot[2][2]) + return plot_gyros_x, plot_gyros_y, plot_gyros_z, + + def gyros_animate(i): + ax_g.set_title(f"Frame {i}") + ax_g.set(xlim=(time_plot[i]-5.0, time_plot[i])) + + plot_gyros_x.set_data(time_plot[:i], gyros_plot[0][:i]) + plot_gyros_y.set_data(time_plot[:i], gyros_plot[1][:i]) + plot_gyros_z.set_data(time_plot[:i], gyros_plot[2][:i]) + return plot_gyros_x, plot_gyros_y, plot_gyros_z, + + gyros_anim = animation.FuncAnimation( + fig_g, + gyros_animate, + init_func=gyros_init, + frames=len(time_plot), + interval=16.6, + blit=True, + repeat=True) + + # Acceleration Figure + fig_a, ax_a = plt.subplots(figsize=(10, 4)) + ax_a.set(xlim=(0, 20), ylim=(-15, 20)) + ax_a.set_xlabel('Time (s)') + ax_a.set_ylabel('Accelerometer (m/s^2)') + ax_a.grid(True) + + plot_acc_x, = ax_a.plot([], [], color='red', label='X') + plot_acc_y, = ax_a.plot([], [], color='green', label='Y') + plot_acc_z, = ax_a.plot([], [], color='blue', label='Z') + + ax_a.legend() + + def acc_init(): + plot_acc_x.set_data(time_plot[2], acc_plot[0][2]) + plot_acc_y.set_data(time_plot[2], acc_plot[1][2]) + plot_acc_z.set_data(time_plot[2], acc_plot[2][2]) + return plot_acc_x, plot_acc_y, plot_acc_z, + + def acc_animate(i): + ax_a.set_title(f"Frame {i}") + ax_a.set(xlim=(time_plot[i]-5.0, time_plot[i])) + + plot_acc_x.set_data(time_plot[:i], acc_plot[0][:i]) + plot_acc_y.set_data(time_plot[:i], acc_plot[1][:i]) + plot_acc_z.set_data(time_plot[:i], acc_plot[2][:i]) + return plot_acc_x, plot_acc_y, plot_acc_z, + + acc_anim = animation.FuncAnimation( + fig_a, + acc_animate, + init_func=acc_init, + frames=len(time_plot), + interval=16.6, + blit=True, + repeat=True) + + # gyros_anim.save('../../../Videos/gyros_plot.mp4', fps=60, extra_args=['-vcodec', 'libx264']) + # acc_anim.save('../../../Videos/acc_plot.mp4', fps=60, extra_args=['-vcodec', 'libx264']) + + plt.show() + + +if __name__ == '__main__': + + main() diff --git a/src/sensor/map_plot.py b/src/sensor/map_plot.py new file mode 100644 index 000000000..24dd77512 --- /dev/null +++ b/src/sensor/map_plot.py @@ -0,0 +1,122 @@ +#!/usr/bin/env python +# 参考：https://github.com/marcgpuig/carla_py_clients/blob/master/map_plot.py + +# Copyright (c) 2019 Marc G Puig. All rights reserved. +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +import glob +import os +import sys + +try: + sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % ( + sys.version_info.major, + sys.version_info.minor, + 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) +except IndexError: + pass + +import carla + +import argparse + +def main(): + argparser = argparse.ArgumentParser() + argparser.add_argument( + '--host', + metavar='H', + default='127.0.0.1', + help='IP of the host server (default: 127.0.0.1)') + argparser.add_argument( + '-p', '--port', + metavar='P', + default=2000, + type=int, + help='TCP port to listen to (default: 2000)') + args = argparser.parse_args() + + # Approximate distance between the waypoints + WAYPOINT_DISTANCE = 5.0 # in meters + + try: + client = carla.Client(args.host, args.port) + client.set_timeout(2.0) + + world = client.get_world() + carla_map = world.get_map() + + import matplotlib.pyplot as plt + + plt.subplot(211) + # Invert the y axis since we follow UE4 coordinates + plt.gca().invert_yaxis() + plt.margins(x=0.7, y=0) + + # GET WAYPOINTS IN THE MAP ########################################## + # Returns a list of waypoints positioned on the center of the lanes + # all over the map with an approximate distance between them. + waypoint_list = carla_map.generate_waypoints(WAYPOINT_DISTANCE) + + plt.plot( + [wp.transform.location.x for wp in waypoint_list], + [wp.transform.location.y for wp in waypoint_list], + linestyle='', markersize=3, color='blue', marker='o') + ##################################################################### + + plt.subplot(212) + # Invert the y axis since we follow UE4 coordinates + plt.gca().invert_yaxis() + plt.margins(x=0.7, y=0) + + # GET WAYPOINTS IN THE MAP ########################################## + # It provides a minimal graph of the topology of the current OpenDRIVE file. + # It is constituted by a list of pairs of waypoints, where the first waypoint + # is the origin and the second one is the destination. + # It can be loaded into NetworkX. + # A valid output could be: [ (w0, w1), (w0, w2), (w1, w3), (w2, w3), (w0, w4) ] + topology = carla_map.get_topology() + road_list = [] + + for wp_pair in topology: + current_wp = wp_pair[0] + # Check if there is a road with no previus road, this can happen + # in opendrive. Then just continue. + if current_wp is None: + continue + # First waypoint on the road that goes from wp_pair[0] to wp_pair[1]. + current_road_id = current_wp.road_id + wps_in_single_road = [current_wp] + # While current_wp has the same road_id (has not arrived to next road). + while current_wp.road_id == current_road_id: + # Check for next waypoints in aprox distance. + available_next_wps = current_wp.next(WAYPOINT_DISTANCE) + # If there is next waypoint/s? + if available_next_wps: + # We must take the first ([0]) element because next(dist) can + # return multiple waypoints in intersections. + current_wp = available_next_wps[0] + wps_in_single_road.append(current_wp) + else: # If there is no more waypoints we can stop searching for more. + break + road_list.append(wps_in_single_road) + + # Plot each road (on a different color by default) + for road in road_list: + plt.plot( + [wp.transform.location.x for wp in road], + [wp.transform.location.y for wp in road]) + ##################################################################### + + plt.show() + + finally: + pass + + +if __name__ == '__main__': + try: + main() + finally: + print('Done.') diff --git a/src/sensor/radar_plotter.py b/src/sensor/radar_plotter.py new file mode 100644 index 000000000..a66bd2e42 --- /dev/null +++ b/src/sensor/radar_plotter.py @@ -0,0 +1,321 @@ +#!/usr/bin/env python +# 参考：https://github.com/marcgpuig/carla_py_clients/blob/master/radar_plotter.py +# 在当前目录下生成*.mp4文件 + +# Copyright (c) 2019 Marc G Puig. All rights reserved. +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +import glob +import os +import sys + +try: + sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % ( + sys.version_info.major, + sys.version_info.minor, + 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) +except IndexError: + pass + +import carla + +import argparse +import math +import time +import weakref + + +def get_correct_type(attr): + if attr.type == carla.ActorAttributeType.Bool: + return attr.as_bool() + if attr.type == carla.ActorAttributeType.Int: + return attr.as_int() + if attr.type == carla.ActorAttributeType.Float: + return attr.as_float() + if attr.type == carla.ActorAttributeType.String: + return attr.as_str() + if attr.type == carla.ActorAttributeType.RGBColor: + return attr.as_color() + + +def main(): + argparser = argparse.ArgumentParser( + description=__doc__) + argparser.add_argument( + '--host', + metavar='H', + default='127.0.0.1', + help='IP of the host server (default: 127.0.0.1)') + argparser.add_argument( + '-p', '--port', + metavar='P', + default=2000, + type=int, + help='TCP port to listen to (default: 2000)') + argparser.add_argument( + '--fps', + metavar='N', + default=30, + type=int, + help='set fixed FPS, zero for variable FPS') + argparser.add_argument( + '--vertical-fov', + metavar='N', + default=30.0, + type=float, + help='radar\'s vertical FOV (default: 30.0)') + argparser.add_argument( + '--horizontal-fov', + metavar='N', + default=30.0, + type=float, + help='radar\'s horizontal FOV (default: 30.0)') + argparser.add_argument( + '--points-per-second', + metavar='N', + default=1500, + type=int, + help='radar\'s total points per second (default: 1500)') + argparser.add_argument( + '--range', + metavar='D', + default=100.0, + type=float, + help='radar\'s maximum range in meters (default: 100.0)') + argparser.add_argument( + '--time', + metavar='S', + default=10, + type=float, + help='time to run in seconds (default: 10)') + args = argparser.parse_args() + + try: + + client = carla.Client(args.host, args.port) + client.set_timeout(2.0) + world = client.get_world() + + settings = world.get_settings() + settings.synchronous_mode = True + settings.fixed_delta_seconds = (1.0 / args.fps) if args.fps > 0.0 else 0.0 + world.apply_settings(settings) + + bp_lb = world.get_blueprint_library() + radar_bp = bp_lb.filter('sensor.other.radar')[0] + + # Set Radar attributes, by default are: + radar_bp.set_attribute('horizontal_fov', str(args.horizontal_fov)) # degrees + radar_bp.set_attribute('vertical_fov', str(args.vertical_fov)) # degrees + radar_bp.set_attribute('points_per_second', str(args.points_per_second)) + radar_bp.set_attribute('range', str(args.range)) # meters + + # Display Radar attributes + print(f"{'- Radar Information ':-<45}") + print(f" - {'ID':<23}{radar_bp.id}") + for attr in radar_bp: + print(f" - {attr.id+':':<23}{get_correct_type(attr)}") + print(f"{'':-<45}") + + mustang_bp = bp_lb.filter('vehicle.ford.mustang')[0] + + start_location = world.get_map().get_spawn_points()[0] + mustang = world.spawn_actor( + mustang_bp, + start_location) + + mustang.set_autopilot(True) + + radar = world.spawn_actor( + radar_bp, + carla.Transform(location=carla.Location(y=1.5, z=0.5)), + mustang) + + camera_bp = bp_lb.filter('sensor.camera.rgb')[0] + image_size = (600, 400) + camera_bp.set_attribute('image_size_x', str(image_size[0])) + camera_bp.set_attribute('image_size_y', str(image_size[1])) + camera = world.spawn_actor( + camera_bp, + carla.Transform(location=carla.Location(y=1.5, z=0.5)), + mustang) + + # Ploting variables + p_depth = [] + p_azimuth = [] + p_altitude = [] + p_velocity = [] + p_images = [] + + def radar_callback(weak_radar, sensor): + self = weak_radar() + if not self: + return + + current_depth = [] + current_azimuth = [] + current_altitude = [] + current_velocity = [] + + for i in sensor: + current_depth.append(i.depth) + current_azimuth.append(i.azimuth) + current_altitude.append(i.altitude) + current_velocity.append(i.velocity) + + p_depth.append(current_depth) + p_azimuth.append(current_azimuth) + p_altitude.append(current_altitude) + p_velocity.append(current_velocity) + + weak_radar = weakref.ref(radar) + radar.listen(lambda sensor: radar_callback(weak_radar, sensor)) + + def camera_callback(weak_camera, image): + self = weak_camera() + if not self: + return + # p_images.append(image) + image.save_to_disk('_out/%08d' % image.frame) + + weak_camera = weakref.ref(camera) + camera.listen(lambda image: camera_callback(weak_camera, image)) + + time_to_run = args.time # in seconds + initial_time = time.time() + close_time = initial_time + time_to_run + + while time.time() < close_time: + sys.stdout.write(f"\rElapsed time: {time.time() - initial_time:4.1f}/{time_to_run} s") + sys.stdout.flush() + world.tick() + + finally: + print('') + try: + settings.synchronous_mode = False + settings.fixed_delta_seconds = None + world.apply_settings(settings) + except NameError: + pass + + try: + radar.destroy() + except NameError: + pass + + try: + camera.destroy() + except NameError: + pass + + try: + mustang.destroy() + except NameError: + pass + + import numpy as np + import matplotlib.pyplot as plt + from matplotlib import animation + + p_depth = np.array([np.array(i) for i in p_depth]) + p_azimuth = np.array([np.array(i) for i in p_azimuth]) + p_altitude = np.array([np.array(i) for i in p_altitude]) + p_velocity = np.array([np.array(i) for i in p_velocity]) + p_images = np.array([np.array(i.raw_data) for i in p_images]) + + # im = np.frombuffer(p_images[5], dtype=np.dtype("uint8")) + # im = im.reshape((image_size[1], image_size[0], 4)) + # im = im[:, :, :3] + # im = im[:, :, ::-1] + + # plt.imshow(im) + # plt.show() + + # sys.exit(0) + + # Animation ################################################## + fig, (ax_depth, ax_vel) = plt.subplots(nrows=1, ncols=2, figsize=(14, 6)) + + half_hor_fov_to_deg = math.radians(args.horizontal_fov) / 2.0 + half_ver_fov_to_deg = math.radians(args.vertical_fov) / 2.0 + horizontal_axes_limits = (-half_hor_fov_to_deg, half_hor_fov_to_deg) + vertical_axes_limits = (-half_ver_fov_to_deg, half_ver_fov_to_deg) + + # - Depth Axes ---------------------------------- + ax_depth.set(xlim=horizontal_axes_limits, ylim=vertical_axes_limits) + ax_depth.set_title('Depth (m)') + ax_depth.set_xlabel('Azimuth (rad)') + ax_depth.set_ylabel('Altitude (rad)') + + # Initialize the depth scater plot without data + scat_depth = ax_depth.scatter( + np.array([]), + np.array([]), + c=np.array([]), # dot intensity/color + s=20, # dot size + vmin=0.0, # colorize from 0 m + vmax=args.range, # to Radar's maximum disance (far) + cmap='viridis') # viridis, plasma, gray... + + fig.colorbar(scat_depth, ax=ax_depth, extend='max', shrink=1.0, pad=0.01) + + # - Velocity Axes ------------------------------- + ax_vel.set(xlim=horizontal_axes_limits, ylim=vertical_axes_limits) + ax_vel.set_title('Velocity (m/s)') + ax_vel.set_xlabel('Azimuth (rad)') + ax_vel.set_ylabel('Altitude (rad)') + + # Initialize the velocity scater plot without data + scat_vel = ax_vel.scatter( + np.array([]), + np.array([]), + c=np.array([]), # dot intensity/color + s=20, # dot size + vmin=-10.0, # colorize from -10 m/s + vmax=10.0, # to +10 m/s + cmap='RdBu') # RdBu, Spectral, coolwarm... + + fig.colorbar(scat_vel, ax=ax_vel, extend='both', shrink=1.0, pad=0.01) + + # Animation initialization callback + def acc_init(): + scat_depth.set_array(p_depth[0]) + scat_vel.set_array(p_velocity[0]) + + scat_depth.set_offsets(np.c_[p_azimuth[0], p_altitude[0]]) + scat_vel.set_offsets(np.c_[p_azimuth[0], p_altitude[0]]) + + return scat_depth, scat_vel + + # Animation update callback + def radar_anim_update(i): + scat_depth.set_array(p_depth[i]) + scat_vel.set_array(p_velocity[i]) + + scat_depth.set_offsets(np.c_[p_azimuth[i], p_altitude[i]]) + scat_vel.set_offsets(np.c_[p_azimuth[i], p_altitude[i]]) + + return scat_depth, scat_vel + + # Do the animation + radar_animation = animation.FuncAnimation( + fig, + radar_anim_update, + init_func=acc_init, + frames=len(p_azimuth), + interval=(1.0 / args.fps) if args.fps > 0.0 else 0.0, + blit=True, + repeat=True) + + # Store the animation as mp4 + radar_animation.save( + f"./radar_plot{time.time()}.mp4", + fps=args.fps, + extra_args=["-vcodec", "libx264"]) + ############################################################## + +if __name__ == '__main__': + main() diff --git a/src/walker/pedestrian_eye.py b/src/walker/pedestrian_eye.py new file mode 100644 index 000000000..1bd892313 --- /dev/null +++ b/src/walker/pedestrian_eye.py @@ -0,0 +1,251 @@ +# 生成一个行人并获取眼睛里的图像 +# 1. 生成行人 +# 2. 将相机挂载到眼眶中； +# 3. 获取双目相机的图像； +from __future__ import annotations + +import argparse +import glob +import os +import random +import sys + +import cv2 +import numpy as np + +sys.path.append(os.path.join(os.path.dirname(os.path.abspath(__file__)), "..")) + +try: + sys.path.append( + glob.glob( + "%s/PythonAPI/carla/dist/carla-*%d.%d-%s.egg" + % ( + os.environ.get("CARLA_ROOT", "."), + sys.version_info.major, + sys.version_info.minor, + "win-amd64" if os.name == "nt" else "linux-x86_64", + ) + )[0] + ) +except IndexError: + pass + +import carla + +from carla_world import CarlaWorld + + +def initialize_world(args): + """创建世界实例并创建/获取英雄参与者""" + client = carla.Client(args.host, args.port) + carla_world = CarlaWorld(client) + if args.map: + carla_world.load_world(args.map) + + hero = carla_world.find_actor_by_name(args.rolename) + if hero is None: + # 生成行人 + hero = carla_world.spawn_actor(args.rolename, "*walker.pedestrian*") + + return carla_world, hero + + +# 生成RGB相机 +def spawn_rgb_cameras( + carla_world: CarlaWorld, hero: carla.Vehicle, image_size_x: int, image_size_y: int +): + camera_init_trans = carla.Transform(carla.Location(z=2)) + + # 生成RGB相机传感器 + camera = carla_world.spawn_sensor( + "sensor.camera.rgb", + hero, + camera_init_trans, + image_size_x=image_size_x, + image_size_y=image_size_y, + ) + # 生成语义分割相机 + sem_camera = carla_world.spawn_sensor( + "sensor.camera.semantic_segmentation", + hero, + camera_init_trans, + image_size_x=image_size_x, + image_size_y=image_size_y, + ) + # 生成实例分割相机传感器 + inst_camera = carla_world.spawn_sensor( + "sensor.camera.instance_segmentation", + hero, + camera_init_trans, + image_size_x=image_size_x, + image_size_y=image_size_y, + ) + # 生成深度相机 + depth_camera = carla_world.spawn_sensor( + "sensor.camera.depth", + hero, + camera_init_trans, + image_size_x=image_size_x, + image_size_y=image_size_y, + ) + # 生成动态视觉传感器 + dvs_camera = carla_world.spawn_sensor( + "sensor.camera.dvs", + hero, + camera_init_trans, + image_size_x=image_size_x, + image_size_y=image_size_y, + ) + # 生成光流相机 + opt_camera = carla_world.spawn_sensor( + "sensor.camera.optical_flow", + hero, + camera_init_trans, + image_size_x=image_size_x, + image_size_y=image_size_y, + ) + + camera_dict = { + "rgb": camera, + "semantic": sem_camera, + "instance": inst_camera, + "depth": depth_camera, + "dvs": dvs_camera, + "optical": opt_camera, + } + return camera_dict + + +def setup_callbacks(camera_dict: dict[str, carla.Sensor]): + sensor = next(iter(camera_dict.values())) + image_w = int(sensor.attributes["image_size_x"]) + image_h = int(sensor.attributes["image_size_y"]) + sensor_data = { + "rgb_image": np.zeros((image_h, image_w, 4)), + "sem_image": np.zeros((image_h, image_w, 4)), + "depth_image": np.zeros((image_h, image_w, 4)), + "dvs_image": np.zeros((image_h, image_w, 4)), + "opt_image": np.zeros((image_h, image_w, 4)), + "inst_image": np.zeros((image_h, image_w, 4)), + } + + # RGB相机的回调函数 + def rgb_callback(image: carla.Image, data_dict: dict): + data_dict["rgb_image"] = np.reshape( + np.copy(image.raw_data), (image.height, image.width, 4) + ) + + def sem_callback(image: carla.Image, data_dict: dict): + image.convert(carla.ColorConverter.CityScapesPalette) + data_dict["sem_image"] = np.reshape( + np.copy(image.raw_data), (image.height, image.width, 4) + ) + + def inst_callback(image: carla.Image, data_dict: dict): + data_dict["inst_image"] = np.reshape( + np.copy(image.raw_data), (image.height, image.width, 4) + ) + + def depth_callback(image: carla.Image, data_dict: dict): + image.convert(carla.ColorConverter.LogarithmicDepth) + data_dict["depth_image"] = np.reshape( + np.copy(image.raw_data), (image.height, image.width, 4) + ) + + def opt_callback(data: carla.Image, data_dict: dict): + image = data.get_color_coded_flow() + img = np.reshape(np.copy(image.raw_data), (image.height, image.width, 4)) + img[:, :, 3] = 255 + data_dict["opt_image"] = img + + def dvs_callback(data: carla.Image, data_dict: dict): + dvs_events = np.frombuffer( + data.raw_data, + dtype=np.dtype( + [("x", np.uint16), ("y", np.uint16), ("t", np.int64), ("pol", bool)] + ), + ) + data_dict["dvs_image"] = np.zeros((data.height, data.width, 4), dtype=np.uint8) + dvs_img = np.zeros((data.height, data.width, 3), dtype=np.uint8) + dvs_img[dvs_events[:]["y"], dvs_events[:]["x"], dvs_events[:]["pol"] * 2] = 255 + data_dict["dvs_image"][:, :, 0:3] = dvs_img + + camera_dict["rgb"].listen(lambda image: rgb_callback(image, sensor_data)) + camera_dict["semantic"].listen(lambda image: sem_callback(image, sensor_data)) + camera_dict["instance"].listen(lambda image: inst_callback(image, sensor_data)) + camera_dict["depth"].listen(lambda image: depth_callback(image, sensor_data)) + camera_dict["dvs"].listen(lambda image: dvs_callback(image, sensor_data)) + camera_dict["optical"].listen(lambda image: opt_callback(image, sensor_data)) + return sensor_data + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("--host", type=str, default="localhost") + parser.add_argument("--port", type=int, default=2000) + parser.add_argument("--map", type=str, default=None) + parser.add_argument("--rolename", type=str, default="hero") + parser.add_argument("--image_size_x", type=int, default=256) + parser.add_argument("--image_size_y", type=int, default=256) + parser.add_argument("--keep_ego_vehicle", action="store_true") + args = parser.parse_args() + + carla_world, hero = initialize_world(args) + # 生成6个不同相机 + camera_dict = spawn_rgb_cameras( + carla_world, hero, args.image_size_x, args.image_size_y + ) + sensor_data = setup_callbacks(camera_dict) + + # 在 OpenCV 窗口中显示的名字 + cv2.namedWindow("All cameras", cv2.WINDOW_AUTOSIZE) + + # 在一个数组中平铺所有的数据 + top_row = np.concatenate( + (sensor_data["rgb_image"], sensor_data["sem_image"], sensor_data["inst_image"]), + axis=1, + ) + lower_row = np.concatenate( + ( + sensor_data["depth_image"], + sensor_data["dvs_image"], + sensor_data["opt_image"], + ), + axis=1, + ) + tiled = np.concatenate((top_row, lower_row), axis=0) + + # 使用 imshow 函数显示 + cv2.imshow("All cameras", tiled) + cv2.waitKey(1) + + try: + while True: + # 将相机图像平铺进一个数组 + top_row = np.concatenate( + ( + sensor_data["rgb_image"], + sensor_data["sem_image"], + sensor_data["inst_image"], + ), + axis=1, + ) + lower_row = np.concatenate( + ( + sensor_data["depth_image"], + sensor_data["dvs_image"], + sensor_data["opt_image"], + ), + axis=1, + ) + tiled = np.concatenate((top_row, lower_row), axis=0) + cv2.imshow("All cameras", tiled) + + if cv2.waitKey(1) == ord("q"): + break + finally: + cv2.destroyAllWindows() + + if args.keep_ego_vehicle: + carla_world.unregister_actor(hero) + carla_world.cleanup()