• 在CARLA中手动开车，添加双目相机stereo camera，激光雷达Lidar

    CARLA的使用逻辑：

    首先创建客户端

    设置如果2秒没有从服务器返回任何内容，则终止

    1. client = carla.Client("127.0.0.1", 2000)
    2. client.set_timeout(2.0)

    从客户端中get world

    world = client.get_world()

    设置setting并应用

    这里使用固定时间步长的异步模式

    1. settings = world.get_settings()
    2. settings.synchronous_mode = False    # Enables synchronous mode
    3. settings.fixed_delta_seconds = None
    4. delta = 0.05
    5. settings.fixed_delta_seconds = delta
    6. world.apply_settings(settings)

    设置traffic manager

    1. traffic_manager = client.get_trafficmanager(8000)
    2. traffic_manager.set_synchronous_mode(False)
    3. traffic_manager.set_random_device_seed(0)
    4. random.seed(0)

    创建一个观察视角

    spectator是我们观察的视角，在后面的循环中，我们让视角始终跟随车，显示从上往下看的车的俯视图

    spectator = world.get_spectator()

     从map中获得所有可行的出生点

    spawn_points是所有可行的出生点，在后面生成车的时候要指定一个出生点。

    spawn_points = world.get_map().get_spawn_points()

    carla中添加任何东西（车辆，相机，激光雷达等）的逻辑是：

    以车为例，添加车的逻辑是：先从world中get所有东西的blueprint库，从所有blueprint库中filter车的blueprint，再将车的blueprint与出生点结合，生成一个车。

    1. blueprint_library = world.get_blueprint_library()
    2. vehicle_bp = blueprint_library.filter("model3")[0]
    3. vehicle = world.try_spawn_actor(vehicle_bp, spawn_points[0])

    blueprint_library是所有blueprint的库，vehicle_bp是车的blueprint，与一个出生点结合，生成车的实例vehicle。

    相机也是同样的逻辑：

    1. camera_init_trans = carla.Transform(carla.Location(x=1, y=-1, z=2), carla.Rotation(pitch=0))
    2. camera_bp = blueprint_library.find('sensor.camera.rgb')
    3. camera_bp.set_attribute("image_size_x", str(1280))
    4. camera_bp.set_attribute("image_size_y", str(720))
    5. camera_bp.set_attribute("fov", str(105))
    6. camera = world.spawn_actor(camera_bp, camera_init_trans, attach_to=vehicle)

    相机要设置附着在车上面，carla.Location是固定在车身的随车坐标系，原点的高度在车轮与地面接触的平面，俯视图看原点在车的中心。x轴正方向指向车头，y轴指向车辆右舷，z轴指向天空，单位是米。

    相机监听回调函数：

    1. # Get camera dimensions
    2. image_w = camera_bp.get_attribute("image_size_x").as_int()
    3. image_h = camera_bp.get_attribute("image_size_y").as_int()
    4.  
    5. # Instantiate objects for rendering and vehicle control
    6. renderObject = RenderObject(image_w, image_h)
    7. controlObject = ControlObject(vehicle)
    8.  
    9. camera.listen(lambda image: pygame_callback1(image, renderObject))

    carla中没有双目相机，需要自己添加两个rgb相机实现双目相机的效果。

    如何理解CARLA中相机的焦距的概念？

    定义相机的时候，没有焦距这个概念，首先定义窗口的大小，也就是相当于相机的传感器横竖各有多少像素，再定义fov视角范围，单位是度，定义好这两个概念，相机的焦距f根据这两个数来算，如果双目相机恢复深度，需要这样去算f。假如相机的归一化平面，即距离相机z=1米的位置（这个z是slam相机坐标系，不是carla坐标系）有一根横棒，那么多长的横棒可以正好横向填满相机传感器，根据视角范围fov解相似三角形，求出slam中定义的相机内参概念f，窗口大小的一般，也就是slam中相机内参的c

    键盘控制器：

    见完整代码 ControlObject类

    每一步循环中：

    1. world.tick() # 获取服务器结果
    2. # 获取观察视角
    3. transform = vehicle.get_transform()  # we get the transform of vehicle
    4. spectator.set_transform(carla.Transform(transform.location + carla.Location(z=50),
    5.                                                     carla.Rotation(pitch=-90)))
    6.  
    7. for event in pygame.event.get():  # 获取键盘操作

    完整代码：

    1. import carla
    2. import random
    3. import time
    4. import sys
    5. import pygame
    6. import datetime
    7. import numpy as np
    8. import open3d as o3d
    9. from matplotlib import cm
    10.  
    11.  
    12. VIDIDIS = np.array(cm.get_cmap("plasma").colors)
    13. VID_RANGE = np.linspace(0.0, 1.0, VIDIDIS.shape[0])
    14.  
    15. # Render object to keep and pass the PyGame surface
    16. class RenderObject(object):
    17.     def __init__(self, width, height):
    18.         init_image = np.random.randint(0,255,(height, width, 3),dtype='uint8')
    19.         self.surface = pygame.surfarray.make_surface(init_image.swapaxes(0,1))
    20.  
    21.  
    22. # Control object to manage vehicle controls
    23. class ControlObject(object):
    24.     def __init__(self, veh):
    25.  
    26.         # Conrol parameters to store the control state
    27.         self._vehicle = veh
    28.         self._steer = 0
    29.         self._throttle = False
    30.         self._brake = False
    31.         self._steer = None
    32.         self._steer_cache = 0
    33.         # A carla.VehicleControl object is needed to alter the
    34.         # vehicle's control state
    35.         self._control = carla.VehicleControl()
    36.  
    37.     # Check for key press events in the PyGame window
    38.     # and define the control state
    39.     def parse_control(self, event):
    40.         if event.type == pygame.KEYDOWN:
    41.             if event.key == pygame.K_RETURN:
    42.                 self._vehicle.set_autopilot(True)
    43.             if event.key == pygame.K_UP:
    44.                 self._throttle = True
    45.             if event.key == pygame.K_DOWN:
    46.                 self._brake = True
    47.             if event.key == pygame.K_RIGHT:
    48.                 self._steer = 1
    49.             if event.key == pygame.K_LEFT:
    50.                 self._steer = -1
    51.         if event.type == pygame.KEYUP:
    52.             if event.key == pygame.K_UP:
    53.                 self._throttle = False
    54.             if event.key == pygame.K_DOWN:
    55.                 self._brake = False
    56.                 self._control.reverse = False
    57.             if event.key == pygame.K_RIGHT:
    58.                 self._steer = None
    59.             if event.key == pygame.K_LEFT:
    60.                 self._steer = None
    61.  
    62.     # Process the current control state, change the control parameter
    63.     # if the key remains pressed
    64.     def process_control(self):
    65.         if self._throttle:
    66.             self._control.throttle = min(self._control.throttle + 0.01, 1)
    67.             self._control.gear = 1
    68.             self._control.brake = False
    69.         elif not self._brake:
    70.             self._control.throttle = 0.0
    71.  
    72.         if self._brake:
    73.             # If the down arrow is held down when the car is stationary, switch to reverse
    74.             if self._vehicle.get_velocity().length() < 0.01 and not self._control.reverse:
    75.                 self._control.brake = 0.0
    76.                 self._control.gear = 1
    77.                 self._control.reverse = True
    78.                 self._control.throttle = min(self._control.throttle + 0.1, 1)
    79.             elif self._control.reverse:
    80.                 self._control.throttle = min(self._control.throttle + 0.1, 1)
    81.             else:
    82.                 self._control.throttle = 0.0
    83.                 self._control.brake = min(self._control.brake + 0.3, 1)
    84.         else:
    85.             self._control.brake = 0.0
    86.  
    87.         if self._steer is not None:
    88.             if self._steer == 1:
    89.                 self._steer_cache += 0.03
    90.             if self._steer == -1:
    91.                 self._steer_cache -= 0.03
    92.             min(0.7, max(-0.7, self._steer_cache))
    93.             self._control.steer = round(self._steer_cache,1)
    94.         else:
    95.             if self._steer_cache > 0.0:
    96.                 self._steer_cache *= 0.2
    97.             if self._steer_cache < 0.0:
    98.                 self._steer_cache *= 0.2
    99.             if 0.01 > self._steer_cache > -0.01:
    100.                 self._steer_cache = 0.0
    101.             self._control.steer = round(self._steer_cache,1)
    102.  
    103.         # Ápply the control parameters to the ego vehicle
    104.         self._vehicle.apply_control(self._control)
    105.  
    106.  
    107. def generate_lidar_bp(blueprint_library, delta):
    108.     """
    109.     To get lidar bp
    110.     :param blueprint_library: the world blueprint_library
    111.     :param delta: update rate(s)
    112.     :return: lidar bp
    113.     """
    114.     lidar_bp = blueprint_library.find("sensor.lidar.ray_cast")
    115.     lidar_bp.set_attribute("dropoff_general_rate", "0.0")
    116.     lidar_bp.set_attribute("dropoff_intensity_limit", "1.0")
    117.     lidar_bp.set_attribute("dropoff_zero_intensity", "0.0")
    118.  
    119.     lidar_bp.set_attribute("upper_fov", str(15.0))
    120.     lidar_bp.set_attribute("lower_fov", str(-25.0))
    121.     lidar_bp.set_attribute("channels", str(64.0))
    122.     lidar_bp.set_attribute("range", str(100.0))
    123.     lidar_bp.set_attribute("rotation_frequency", str(1.0 / delta))
    124.     lidar_bp.set_attribute("points_per_second", str(500000))
    125.  
    126.     return lidar_bp
    127.  
    128.  
    129. def lidar_callback(point_cloud, point_list):
    130.     # We need to convert point cloud(carla-format) into numpy.ndarray
    131.     data = np.copy(np.frombuffer(point_cloud.raw_data, dtype = np.dtype("f4")))
    132.     data = np.reshape(data, (int(data.shape[0] / 4), 4))
    133.  
    134.     intensity = data[:, -1]
    135.     intensity_col = 1.0 - np.log(intensity) / np.log(np.exp(-0.004 * 100))
    136.     int_color = np.c_[
    137.         np.interp(intensity_col, VID_RANGE, VIDIDIS[:, 0]),
    138.         np.interp(intensity_col, VID_RANGE, VIDIDIS[:, 1]),
    139.         np.interp(intensity_col, VID_RANGE, VIDIDIS[:, 2])]
    140.  
    141.     points = data[:, :-1] # we only use x, y, z coordinates
    142.     points[:, 1] = -points[:, 1] # This is different from official script
    143.     point_list.points = o3d.utility.Vector3dVector(points)
    144.     point_list.colors = o3d.utility.Vector3dVector(int_color)
    145.  
    146.  
    147. # Camera sensor callback, reshapes raw data from camera into 2D RGB and applies to PyGame surface
    148. def pygame_callback1(data, obj):
    149.     img = np.reshape(np.copy(data.raw_data), (data.height, data.width, 4))
    150.     img = img[:, :, :3]
    151.     img = img[:, :, ::-1]
    152.     # msg = bridge.cv2_to_imgmsg(img, encoding="bgr8")
    153.  
    154.     obj.surface = pygame.surfarray.make_surface(img.swapaxes(0, 1))
    155.     # pub.publish(msg)
    156.  
    157.     print("taking photos1")
    158.  
    159.  
    160. def pygame_callback2(data, obj):
    161.     img = np.reshape(np.copy(data.raw_data), (data.height, data.width, 4))
    162.     img = img[:, :, :3]
    163.     img = img[:, :, ::-1]
    164.     # msg = bridge.cv2_to_imgmsg(img, encoding="bgr8")
    165.  
    166.     # pub2.publish(msg)
    167.  
    168.     print("taking photos2")
    169.  
    170.  
    171. client = carla.Client("127.0.0.1", 2000)
    172. client.set_timeout(2.0)
    173. world = client.get_world()
    174.  
    175. settings = world.get_settings()
    176. settings.synchronous_mode = False    # Enables synchronous mode
    177. settings.fixed_delta_seconds = None
    178. delta = 0.05
    179. settings.fixed_delta_seconds = delta
    180. world.apply_settings(settings)
    181.  
    182. traffic_manager = client.get_trafficmanager(8000)
    183. traffic_manager.set_synchronous_mode(False)
    184. traffic_manager.set_random_device_seed(0)
    185. random.seed(0)
    186.  
    187. # We will set up the spectator so we can see what we do
    188. spectator = world.get_spectator()
    189. # Retrieve the map's spawn points
    190. spawn_points = world.get_map().get_spawn_points()
    191.  
    192. blueprint_library = world.get_blueprint_library()
    193. vehicle_bp = blueprint_library.filter("model3")[0]
    194. vehicle = world.try_spawn_actor(vehicle_bp, spawn_points[0])
    195. vehicle.set_autopilot(False)
    196.  
    197. # Initialise the camera floating behind the vehicle
    198. camera_init_trans = carla.Transform(carla.Location(x=1, y=-1, z=2), carla.Rotation(pitch=0))
    199. camera_bp = blueprint_library.find('sensor.camera.rgb')
    200. camera_bp.set_attribute("image_size_x", str(1280))
    201. camera_bp.set_attribute("image_size_y", str(720))
    202. camera_bp.set_attribute("fov", str(105))
    203. camera = world.spawn_actor(camera_bp, camera_init_trans, attach_to=vehicle)
    204. # Start camera with PyGame callback
    205.  
    206. camera2_init_trans = carla.Transform(carla.Location(x=1, y=1, z=2), carla.Rotation(pitch=0))
    207. camera2_bp = blueprint_library.find('sensor.camera.rgb')
    208. camera2_bp.set_attribute("image_size_x", str(1280))
    209. camera2_bp.set_attribute("image_size_y", str(720))
    210. camera2_bp.set_attribute("fov", str(105))
    211. camera2 = world.spawn_actor(camera2_bp, camera2_init_trans, attach_to=vehicle)
    212.  
    213. # Get camera dimensions
    214. image_w = camera_bp.get_attribute("image_size_x").as_int()
    215. image_h = camera_bp.get_attribute("image_size_y").as_int()
    216.  
    217. # Instantiate objects for rendering and vehicle control
    218. renderObject = RenderObject(image_w, image_h)
    219. controlObject = ControlObject(vehicle)
    220.  
    221. camera.listen(lambda image: pygame_callback1(image, renderObject))
    222.  
    223. lidar_bp = generate_lidar_bp(blueprint_library, delta)
    224. lidar_transform = carla.Transform(carla.Location(x=-0.5, z=1.8))
    225. lidar = world.spawn_actor(lidar_bp, lidar_transform, attach_to=vehicle)
    226.  
    227. point_list = o3d.geometry.PointCloud()
    228.  
    229. lidar.listen(lambda data: lidar_callback(data, point_list))
    230.  
    231. vis = o3d.visualization.Visualizer()
    232. vis.create_window(
    233.     window_name= "Display Point Cloud",
    234.     width= 960,
    235.     height= 540,
    236.     left= 480,
    237.     top= 270)
    238.  
    239. vis.get_render_option().background_color = [0.05, 0.05, 0.05]
    240. vis.get_render_option().point_size = 1
    241. vis.get_render_option().show_coordinate_frame = True
    242.  
    243. frame = 0
    244. dt0 = datetime.datetime.now()
    245.  
    246. # Initialise the display
    247. pygame.init()
    248. gameDisplay = pygame.display.set_mode((image_w,image_h), pygame.HWSURFACE | pygame.DOUBLEBUF)
    249. # Draw black to the display
    250. gameDisplay.fill((0,0,0))
    251. gameDisplay.blit(renderObject.surface, (0,0))
    252. pygame.display.flip()
    253.  
    254. crashed = False
    255. while not crashed:
    256.     # if frame == 2:
    257.     #     vis.add_geometry(point_list)
    258.     #
    259.     # vis.update_geometry(point_list)
    260.     # vis.poll_events()
    261.     # vis.update_renderer()
    262.     # time.sleep(0.005)
    263.  
    264.     world.tick()
    265.  
    266.     transform = vehicle.get_transform()  # we get the transform of vehicle
    267.     spectator.set_transform(carla.Transform(transform.location + carla.Location(z=50),
    268.                                                     carla.Rotation(pitch=-90)))
    269.  
    270.     # Update the display
    271.     gameDisplay.blit(renderObject.surface, (0, 0))
    272.     pygame.display.flip()
    273.  
    274.     controlObject.process_control()
    275.     for event in pygame.event.get():
    276.         if event.type == pygame.QUIT:
    277.             crashed = True
    278.  
    279.         controlObject.parse_control(event)
    280.  
    281.     process_time = datetime.datetime.now() - dt0
    282.     sys.stdout.write("\r" + "FPS: " + str(1.0 / process_time.total_seconds()) + "Current Frame: " + str(frame))
    283.     sys.stdout.flush()
    284.     dt0 = datetime.datetime.now()
    285.  
    286.     frame += 1
    287.  
    288. # Stop camera and quit PyGame after exiting game loop
    289. camera.stop()
    290. camera2.stop()
    291. pygame.quit()

    这里有一个bug，我的代码里分别有Lidar点云可视化的代码和相机照片可视化的代码，可是这两个可视化不能共存，如果Lidar的点云可视化更新，相机图片可视化就不更新，相机更新，Lidar就不更新。不知道为什么，知道的大神欢迎留言。

  • 相关阅读:
    Postgresql源码（77）plpgsql中参数传递和赋值
    java计算机毕业设计vue健身食谱系统源码+mysql数据库+系统+lw文档+部署
    c# 画球
    Spring Security实现基于RBAC的权限表达式动态访问控制
    springboot jpa 返回自定义非实体VO类
    Java备忘录模式剖析及使用场景
    循环分批从数据库获取数据
    [车联网安全自学篇] 五十八. Android安全之APK内存敏感信息泄露挖掘【静态分析】
    烟台专利申请发明-个人发明专利申请条件
    C#WPF控件Button详解
  • 原文地址：https://blog.csdn.net/qq_41816368/article/details/134038385