CARLA 仿真小车、附带左右两个摄像头，一个车顶激光雷达

#!/usr/bin/env python

# Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de
# Barcelona (UAB).
#
# This work is licensed under the terms of the MIT license.
# For a copy, see <https://opensource.org/licenses/MIT>.

import glob
import os
import sys
import numpy as np
import cv2
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])
##寻找启动的版本的python
except IndexError:
    pass

import carla

import random
import time
IM_WIDTH = 640
IM_HEIGHT = 480
SHOW_PREVIEW = False
actor_list = []
def process_img(image, window_name):
    i = np.array(image.raw_data)
   # print(i.shape)
    i2 = i.reshape((IM_HEIGHT, IM_WIDTH, 4))
    i3 = i2[:, :, :3]
    cv2.imshow(window_name, i3)
    cv2.waitKey(1)
    return i3/255.0
# Define a callback function to process camera images
def process_image(image, window_name):
    # Convert the image to a numpy array
    array = np.frombuffer(image.raw_data, dtype=np.uint8).reshape((image.height, image.width, 4))

    # Remove the alpha channel
    array = array[:, :, :3]

    # Flip the image vertically (OpenCV uses a different coordinate system than Carla)
    array = np.flipud(array)

    # Show the image in the window
    cv2.imshow(window_name, array)

def collision_data(self, event):
    self.collision_hist.append(event)
def add_camera_to_vehicle(vehicle, spawn_point, im_width=640, im_height=480):
    """
    为车辆添加一个摄像头

    :param vehicle: 车辆对象
    :param spawn_point: 摄像头生成的位置
    :param im_width: 摄像头图像的宽度，默认值为 640
    :param im_height: 摄像头图像的高度，默认值为 480
    :return: 返回摄像头对象
    """
    blueprint_library = vehicle.get_world().get_blueprint_library()
    cam_bp = blueprint_library.find("sensor.camera.rgb")
    cam_bp.set_attribute("image_size_x", f"{im_width}")
    cam_bp.set_attribute("image_size_y", f"{im_height}")
    cam_bp.set_attribute("fov", "110")
    camera = vehicle.get_world().spawn_actor(cam_bp, spawn_point, attach_to=vehicle)
    return camera
def add_camera_to_vehicle(vehicle, spawn_point, im_width=640, im_height=480):
    """
    为车辆添加一个摄像头

    :param vehicle: 车辆对象
    :param spawn_point: 摄像头生成的位置
    :param im_width: 摄像头图像的宽度，默认值为 640
    :param im_height: 摄像头图像的高度，默认值为 480
    :return: 返回摄像头对象
    """
    blueprint_library = vehicle.get_world().get_blueprint_library()
    cam_bp = blueprint_library.find("sensor.camera.rgb")
    cam_bp.set_attribute("image_size_x", f"{im_width}")
    cam_bp.set_attribute("image_size_y", f"{im_height}")
    cam_bp.set_attribute("fov", "110")
    camera = vehicle.get_world().spawn_actor(cam_bp, spawn_point, attach_to=vehicle)
    return camera

def add_collision_sensor_to_vehicle(vehicle):
    """
    为车辆添加一个碰撞感应器

    :param vehicle: 车辆对象
    :return: 返回碰撞感应器对象
    """
    blueprint_library = vehicle.get_world().get_blueprint_library()
    collision_bp = blueprint_library.find("sensor.other.collision")
    collision_sensor = vehicle.get_world().spawn_actor(collision_bp, carla.Transform(), attach_to=vehicle)
    return collision_sensor

def process_lidar_measurement(measurement):
    points = np.frombuffer(measurement.raw_data, dtype=np.dtype('f4'))
    points = np.reshape(points, (int(points.shape[0] / 4), 4))
    lidar_data = np.array(points[:, :3])
    # 显示点云图像
    image = np.zeros((600, 800, 3), dtype=np.uint8)
    lidar_data *= 50
    lidar_data += [400, 300, 0]
    lidar_data = lidar_data.astype(np.int32)
    for point in lidar_data:
        cv2.circle(image, (point[0], point[1]), 1, (0, 0, 255), -1)
    cv2.imshow('Lidar data', image)
    cv2.waitKey(1)
def add_lidar_to_vehicle(vehicle, spawn_point, num_points=100000):
    # 定义雷达参数
    blueprint_library = vehicle.get_world().get_blueprint_library()
    lidar_bp = blueprint_library.find("sensor.lidar.ray_cast")
    lidar_bp.set_attribute("channels", "360")  # 360°
    lidar_bp.set_attribute("range", "500")
    lidar_bp.set_attribute("points_per_second", "100000")
    lidar_bp.set_attribute("rotation_frequency", "200")  # 将 rotation_frequency 设置为 40
    lidar_bp.set_attribute("upper_fov", "50")  # 将 upper_fov 设置为 30
    lidar_bp.set_attribute("lower_fov", "-45")
    lidar = vehicle.get_world().spawn_actor(lidar_bp, spawn_point, attach_to=vehicle)
    return lidar


def start():
    client = carla.Client("localhost", 2000)
    client.set_timeout(2.2)
    world = client.get_world()
    world = client.load_world('Town05')
    blueprint_library = world.get_blueprint_library()
    bp = blueprint_library.filter("model3")[0]
    print(bp)
    spawn_point = random.choice(world.get_map().get_spawn_points())
    vehicle = world.spawn_actor(bp, spawn_point)
    # vehicle.set_autopilot(True)
    vehicle.apply_control(carla.VehicleControl(throttle=1.0, steer=0.0))
    actor_list.append(vehicle)
    spawn_point_1 = carla.Transform(carla.Location(x=1.5, y=0, z=2.4))
    camera_1 = add_camera_to_vehicle(vehicle, spawn_point_1)
    spawn_point_2 = carla.Transform(carla.Location(x=-1.5, y=0, z=2.4))
    camera_2 = add_camera_to_vehicle(vehicle, spawn_point_2)
    spawn_point_3 = carla.Transform(carla.Location(x=0, y=0, z=2.5))
    lidar_1 = add_lidar_to_vehicle(vehicle, spawn_point_3)
    # 配置并启动雷达
    actor_list.append(lidar_1)
    col = add_collision_sensor_to_vehicle(vehicle)
    actor_list.append(camera_1)####对于显示图像十分重要#####
    actor_list.append(camera_2)  ####对于显示图像十分重要#####
    actor_list.append(col)
    camera_1.listen(lambda data: process_img(data, "camera_1"))
    camera_2.listen(lambda data: process_img(data, "camera_2"))
    lidar_1.listen(process_lidar_measurement)
    col.listen(lambda event: collision_data(event))
try:
    start()
    time.sleep(100)

finally:
    for actor in actor_list:
        actor.destroy()
    print("ALL cleaned up!!!")