当前位置：首页 > news >正文

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

news 文章来源：https://blog.csdn.net/qq_41816368/article/details/134038385 2025/5/9 2:28:26

CARLA的使用逻辑：

首先创建客户端

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

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

从客户端中get world

world = client.get_world()

设置setting并应用

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

settings = world.get_settings()
settings.synchronous_mode = False    # Enables synchronous mode
settings.fixed_delta_seconds = None
delta = 0.05
settings.fixed_delta_seconds = delta
world.apply_settings(settings)

设置traffic manager

traffic_manager = client.get_trafficmanager(8000)
traffic_manager.set_synchronous_mode(False)
traffic_manager.set_random_device_seed(0)
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与出生点结合，生成一个车。

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

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

相机也是同样的逻辑：

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

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

相机监听回调函数：

# Get camera dimensions
image_w = camera_bp.get_attribute("image_size_x").as_int()
image_h = camera_bp.get_attribute("image_size_y").as_int()# Instantiate objects for rendering and vehicle control
renderObject = RenderObject(image_w, image_h)
controlObject = ControlObject(vehicle)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类

每一步循环中：

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

完整代码：

import carla
import random
import time
import sys
import pygame
import datetime
import numpy as np
import open3d as o3d
from matplotlib import cmVIDIDIS = np.array(cm.get_cmap("plasma").colors)
VID_RANGE = np.linspace(0.0, 1.0, VIDIDIS.shape[0])# Render object to keep and pass the PyGame surface
class RenderObject(object):def __init__(self, width, height):init_image = np.random.randint(0,255,(height, width, 3),dtype='uint8')self.surface = pygame.surfarray.make_surface(init_image.swapaxes(0,1))# Control object to manage vehicle controls
class ControlObject(object):def __init__(self, veh):# Conrol parameters to store the control stateself._vehicle = vehself._steer = 0self._throttle = Falseself._brake = Falseself._steer = Noneself._steer_cache = 0# A carla.VehicleControl object is needed to alter the# vehicle's control stateself._control = carla.VehicleControl()# Check for key press events in the PyGame window# and define the control statedef parse_control(self, event):if event.type == pygame.KEYDOWN:if event.key == pygame.K_RETURN:self._vehicle.set_autopilot(True)if event.key == pygame.K_UP:self._throttle = Trueif event.key == pygame.K_DOWN:self._brake = Trueif event.key == pygame.K_RIGHT:self._steer = 1if event.key == pygame.K_LEFT:self._steer = -1if event.type == pygame.KEYUP:if event.key == pygame.K_UP:self._throttle = Falseif event.key == pygame.K_DOWN:self._brake = Falseself._control.reverse = Falseif event.key == pygame.K_RIGHT:self._steer = Noneif event.key == pygame.K_LEFT:self._steer = None# Process the current control state, change the control parameter# if the key remains presseddef process_control(self):if self._throttle:self._control.throttle = min(self._control.throttle + 0.01, 1)self._control.gear = 1self._control.brake = Falseelif not self._brake:self._control.throttle = 0.0if self._brake:# If the down arrow is held down when the car is stationary, switch to reverseif self._vehicle.get_velocity().length() < 0.01 and not self._control.reverse:self._control.brake = 0.0self._control.gear = 1self._control.reverse = Trueself._control.throttle = min(self._control.throttle + 0.1, 1)elif self._control.reverse:self._control.throttle = min(self._control.throttle + 0.1, 1)else:self._control.throttle = 0.0self._control.brake = min(self._control.brake + 0.3, 1)else:self._control.brake = 0.0if self._steer is not None:if self._steer == 1:self._steer_cache += 0.03if self._steer == -1:self._steer_cache -= 0.03min(0.7, max(-0.7, self._steer_cache))self._control.steer = round(self._steer_cache,1)else:if self._steer_cache > 0.0:self._steer_cache *= 0.2if self._steer_cache < 0.0:self._steer_cache *= 0.2if 0.01 > self._steer_cache > -0.01:self._steer_cache = 0.0self._control.steer = round(self._steer_cache,1)# Ápply the control parameters to the ego vehicleself._vehicle.apply_control(self._control)def generate_lidar_bp(blueprint_library, delta):"""To get lidar bp:param blueprint_library: the world blueprint_library:param delta: update rate(s):return: lidar bp"""lidar_bp = blueprint_library.find("sensor.lidar.ray_cast")lidar_bp.set_attribute("dropoff_general_rate", "0.0")lidar_bp.set_attribute("dropoff_intensity_limit", "1.0")lidar_bp.set_attribute("dropoff_zero_intensity", "0.0")lidar_bp.set_attribute("upper_fov", str(15.0))lidar_bp.set_attribute("lower_fov", str(-25.0))lidar_bp.set_attribute("channels", str(64.0))lidar_bp.set_attribute("range", str(100.0))lidar_bp.set_attribute("rotation_frequency", str(1.0 / delta))lidar_bp.set_attribute("points_per_second", str(500000))return lidar_bpdef lidar_callback(point_cloud, point_list):# We need to convert point cloud(carla-format) into numpy.ndarraydata = np.copy(np.frombuffer(point_cloud.raw_data, dtype = np.dtype("f4")))data = np.reshape(data, (int(data.shape[0] / 4), 4))intensity = data[:, -1]intensity_col = 1.0 - np.log(intensity) / np.log(np.exp(-0.004 * 100))int_color = np.c_[np.interp(intensity_col, VID_RANGE, VIDIDIS[:, 0]),np.interp(intensity_col, VID_RANGE, VIDIDIS[:, 1]),np.interp(intensity_col, VID_RANGE, VIDIDIS[:, 2])]points = data[:, :-1] # we only use x, y, z coordinatespoints[:, 1] = -points[:, 1] # This is different from official scriptpoint_list.points = o3d.utility.Vector3dVector(points)point_list.colors = o3d.utility.Vector3dVector(int_color)# Camera sensor callback, reshapes raw data from camera into 2D RGB and applies to PyGame surface
def pygame_callback1(data, obj):img = np.reshape(np.copy(data.raw_data), (data.height, data.width, 4))img = img[:, :, :3]img = img[:, :, ::-1]# msg = bridge.cv2_to_imgmsg(img, encoding="bgr8")obj.surface = pygame.surfarray.make_surface(img.swapaxes(0, 1))# pub.publish(msg)print("taking photos1")def pygame_callback2(data, obj):img = np.reshape(np.copy(data.raw_data), (data.height, data.width, 4))img = img[:, :, :3]img = img[:, :, ::-1]# msg = bridge.cv2_to_imgmsg(img, encoding="bgr8")# pub2.publish(msg)print("taking photos2")client = carla.Client("127.0.0.1", 2000)
client.set_timeout(2.0)
world = client.get_world()settings = world.get_settings()
settings.synchronous_mode = False    # Enables synchronous mode
settings.fixed_delta_seconds = None
delta = 0.05
settings.fixed_delta_seconds = delta
world.apply_settings(settings)traffic_manager = client.get_trafficmanager(8000)
traffic_manager.set_synchronous_mode(False)
traffic_manager.set_random_device_seed(0)
random.seed(0)# We will set up the spectator so we can see what we do
spectator = world.get_spectator()
# Retrieve the map's spawn points
spawn_points = world.get_map().get_spawn_points()blueprint_library = world.get_blueprint_library()
vehicle_bp = blueprint_library.filter("model3")[0]
vehicle = world.try_spawn_actor(vehicle_bp, spawn_points[0])
vehicle.set_autopilot(False)# Initialise the camera floating behind the vehicle
camera_init_trans = carla.Transform(carla.Location(x=1, y=-1, z=2), carla.Rotation(pitch=0))
camera_bp = blueprint_library.find('sensor.camera.rgb')
camera_bp.set_attribute("image_size_x", str(1280))
camera_bp.set_attribute("image_size_y", str(720))
camera_bp.set_attribute("fov", str(105))
camera = world.spawn_actor(camera_bp, camera_init_trans, attach_to=vehicle)
# Start camera with PyGame callbackcamera2_init_trans = carla.Transform(carla.Location(x=1, y=1, z=2), carla.Rotation(pitch=0))
camera2_bp = blueprint_library.find('sensor.camera.rgb')
camera2_bp.set_attribute("image_size_x", str(1280))
camera2_bp.set_attribute("image_size_y", str(720))
camera2_bp.set_attribute("fov", str(105))
camera2 = world.spawn_actor(camera2_bp, camera2_init_trans, attach_to=vehicle)# Get camera dimensions
image_w = camera_bp.get_attribute("image_size_x").as_int()
image_h = camera_bp.get_attribute("image_size_y").as_int()# Instantiate objects for rendering and vehicle control
renderObject = RenderObject(image_w, image_h)
controlObject = ControlObject(vehicle)camera.listen(lambda image: pygame_callback1(image, renderObject))lidar_bp = generate_lidar_bp(blueprint_library, delta)
lidar_transform = carla.Transform(carla.Location(x=-0.5, z=1.8))
lidar = world.spawn_actor(lidar_bp, lidar_transform, attach_to=vehicle)point_list = o3d.geometry.PointCloud()lidar.listen(lambda data: lidar_callback(data, point_list))vis = o3d.visualization.Visualizer()
vis.create_window(window_name= "Display Point Cloud",width= 960,height= 540,left= 480,top= 270)vis.get_render_option().background_color = [0.05, 0.05, 0.05]
vis.get_render_option().point_size = 1
vis.get_render_option().show_coordinate_frame = Trueframe = 0
dt0 = datetime.datetime.now()# Initialise the display
pygame.init()
gameDisplay = pygame.display.set_mode((image_w,image_h), pygame.HWSURFACE | pygame.DOUBLEBUF)
# Draw black to the display
gameDisplay.fill((0,0,0))
gameDisplay.blit(renderObject.surface, (0,0))
pygame.display.flip()crashed = False
while not crashed:# if frame == 2:#     vis.add_geometry(point_list)## vis.update_geometry(point_list)# vis.poll_events()# vis.update_renderer()# time.sleep(0.005)world.tick()transform = vehicle.get_transform()  # we get the transform of vehiclespectator.set_transform(carla.Transform(transform.location + carla.Location(z=50),carla.Rotation(pitch=-90)))# Update the displaygameDisplay.blit(renderObject.surface, (0, 0))pygame.display.flip()controlObject.process_control()for event in pygame.event.get():if event.type == pygame.QUIT:crashed = TruecontrolObject.parse_control(event)process_time = datetime.datetime.now() - dt0sys.stdout.write("\r" + "FPS: " + str(1.0 / process_time.total_seconds()) + "Current Frame: " + str(frame))sys.stdout.flush()dt0 = datetime.datetime.now()frame += 1# Stop camera and quit PyGame after exiting game loop
camera.stop()
camera2.stop()
pygame.quit()

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