I'm currently working on a project where I'm trying to create a trajectory visualization in a Pygame window, similar to the image shown below:
The concept behind this is to retrieve waypoints from Carla, and then draw a trajectory on the Pygame window. The trajectory should extend from the vehicle's current location to a specific point. This trajectory should also update dynamically as the vehicle moves.
her's what i did
from cameraManager import CameraManager, HUD
import carla
import pygame
import numpy as np
import random
# Initialize CARLA
client = carla.Client('localhost', 2000)
client.set_timeout(2.0)
world = client.get_world()
# Pygame initialization
pygame.init()
# Define the font for the text
pygame.font.init()
font = pygame.font.SysFont(None, 36)
display_width, display_height = 800, 600
display = pygame.display.set_mode(
(display_width, display_height),
pygame.HWSURFACE | pygame.DOUBLEBUF)
pygame.display.set_caption('Waypoints Visualization')
clock = pygame.time.Clock()
hud = HUD(display_width, display_height)
def spawn_vehicle():
"""
This function spawns vehicles at the given spawn points.
It search for a random point that not taken by another vehicle or object
"""
world = client.get_world()
blueprint_library = world.get_blueprint_library()
vehicle_bp = blueprint_library.find('vehicle.audi.a2')
# Set a different spawn point manually
spawn_point = carla.Transform(carla.Location(
x=0, y=0, z=0), carla.Rotation(yaw=0))
while True:
try:
vehicle = world.spawn_actor(vehicle_bp, spawn_point)
break
except Exception as e:
print(f"Spawn failed: {e} Retrying with a different spawn point")
# Generate a new spawn point
spawn_points = world.get_map().get_spawn_points()
spawn_point = random.choice(spawn_points)
print(f"Spawn point: {spawn_point}")
return vehicle
def get_waypoints_ahead(vehicle):
current_map = world.get_map()
waypoint = current_map.get_waypoint(
vehicle.get_location(), project_to_road=True, lane_type=carla.LaneType.Driving)
waypoints = []
for _ in range(8//2):
waypoint = waypoint.next(6)[0]
waypoints.append(waypoint.transform.location)
return waypoints
# Function to transform CARLA's 3D world coordinates to 2D screen coordinates
def world_to_screen(world_location, vehicle_transform, scale=0.5):
# Relative coordinates
# x_rel = world_location.x - vehicle_transform.location.x
# y_rel = world_location.y - vehicle_transform.location.y
# # Take into account the height of the camera on the car
# z_rel = world_location.z - vehicle_transform.location.z
x_rel = vehicle_transform.location.x
y_rel = vehicle_transform.location.y
# Take into account the height of the camera on the car
z_rel = vehicle_transform.location.z
# Rotate to vehicle's perspective
yaw_rad = np.deg2rad(vehicle_transform.rotation.yaw)
x = x_rel * np.cos(yaw_rad) + y_rel * np.sin(yaw_rad)
y = -x_rel * np.sin(yaw_rad) + y_rel * np.cos(yaw_rad)
# Adjust for the camera's height (assuming the camera's height is around 1.5 meters from the ground)
y -= z_rel - 1.5
# Transform to screen space
screen_x = 800 // 2 + int(x * scale)
screen_y = 600 - int(y * scale)
return screen_x, screen_y
def draw_waypoints_on_pygame_display(waypoints, vehicle_transform, display):
for waypoint in waypoints:
screen_x, screen_y = world_to_screen(waypoint, vehicle_transform)
# pygame.draw.circle(display, (0, 255, 0), (vehicle_transform.location.x,vehicle_transform.location.y), 5)
pygame.draw.line(display,(0, 223, 222),(vehicle_transform.location.x,vehicle_transform.location.y),(screen_x,screen_y),6)
print(f"screen_projection = {screen_x} ::: vehicle_location = {vehicle.get_location()}")
vehicle = spawn_vehicle()
camera = CameraManager(vehicle, hud, display)
camera.transform_index = 0 # Choose the index of the camera transform
camera.set_sensor(0)
# Main loop
while True:
for event in pygame.event.get():
if event.type == pygame.QUIT:
pygame.quit()
quit()
camera.render(display)
waypoints = get_waypoints_ahead(vehicle)
print(f"WayPoints_x-->{waypoints} ")
draw_waypoints_on_pygame_display(
waypoints, vehicle.get_transform(), display)
vehicle.set_autopilot(True)
pygame.display.flip()
clock.tick(60)
any idea or help to do it
