from abc import ABC, abstractmethod
from dataclasses import dataclass, field
from itertools import product
from typing import TYPE_CHECKING
import carla
import numpy as np
if TYPE_CHECKING:
from .abstraction import GridMap
[docs]
@dataclass(frozen=True)
class Gizmo(ABC):
"""Abstract base class for debug visualization gizmos in CARLA.
Gizmos are used to display debug information in the CARLA simulator
such as points, grids, paths, and other geometric shapes.
"""
color: "carla.Color" = field(default_factory=carla.Color)
thickness: float = field(default=0.06)
life_time: float = field(default=1.0)
[docs]
@abstractmethod
def draw(self, world: "carla.World"):
"""Draw the gizmo in the world.
Args
----
world : carla.World
The CARLA world to draw the gizmo in
"""
[docs]
def initialise(self, world: "carla.World"):
"""
Initialise the gizmo with the world.
The world is the Carla world that the gizmo is associated with.
"""
assert isinstance(world, carla.World), f"Expected carla.World, got {type(world)}"
self.draw(world)
[docs]
@dataclass(frozen=True)
class PointGizmo(Gizmo):
"""A gizmo that draws a single point at a location.
Displays a point with the specified size and color at a given location.
"""
location: "carla.Location" = field(default_factory=carla.Location)
size: float = field(default=0.5)
def __post_init__(self):
if not isinstance(self.location, carla.Location):
object.__setattr__(self, "location", carla.Location(*self.location))
[docs]
def update_location(self, new_location: "carla.Location"):
if not isinstance(new_location, carla.Location):
new_location = carla.Location(*new_location)
object.__setattr__(self, "location", new_location)
[docs]
def update_color(self, new_color: "carla.Color"):
if not isinstance(new_color, carla.Color):
raise ValueError(f"Expected carla.Color, got {type(new_color)}")
object.__setattr__(self, "color", new_color)
[docs]
def draw(self, world: "carla.World"):
world.debug.draw_point(self.location, self.size, self.color, self.life_time)
[docs]
@dataclass(frozen=True)
class GridGizmo(Gizmo):
"""A gizmo that draws a regular grid.
Displays a grid of boxes with the specified origin, shape, and cell size.
"""
origin: "carla.Location" = field(default_factory=carla.Location)
shape: "tuple[int, int]" = field(default=(1, 1))
cell_size: "tuple[float, float]" = field(default=(1.0, 1.0))
thickness: float = field(default=0.06)
def __post_init__(self):
if isinstance(self.origin, (np.ndarray, list, tuple)):
object.__setattr__(self, "origin", carla.Location(*self.origin))
[docs]
def draw(self, world: "carla.World"):
ox, oy, oz = self.origin.x, self.origin.y, self.origin.z
extent = carla.Vector3D(self.cell_size[0] / 2, self.cell_size[1] / 2, 0)
rotation = carla.Rotation(0, 0, 0)
for x, y in product(range(self.shape[0]), range(self.shape[1])):
corner = carla.Location(ox + extent.x + x * self.cell_size[0], oy + extent.y + y * self.cell_size[1], oz)
world.debug.draw_box(
carla.BoundingBox(corner, extent),
rotation=rotation,
thickness=self.thickness,
life_time=self.life_time,
color=self.color,
)
# world.debug.draw_string(
# corner, f"({corner.x}, {corner.y})", draw_shadow=False, color=carla.Color(), life_time=600
# )
# world.debug.draw_point(location=corner, size=0.1, color=carla.Color(255), life_time=self.life_time)
[docs]
@dataclass(frozen=True)
class GridMapGizmo(Gizmo):
"""A gizmo that draws a GridMap visualization.
Displays an occupancy grid map with cells colored based on occupancy status.
Optionally shows occupied cell positions and coordinates.
"""
grid_map: "GridMap" = field(default=None)
thickness: float = field(default=0.09)
z: float = field(default=0.0)
draw_pos: bool = field(default=False)
_all_centers: "tuple[carla.Location]" = field(init=False, default=None)
_all_pos: "tuple[carla.Location]" = field(init=False, default_factory=tuple)
_extent: "carla.Vector3D" = field(init=False, default=None)
_rotation: "carla.Rotation" = field(init=False, default=carla.Rotation())
_corners: "tuple[carla.Location, carla.Location, carla.Location, carla.Location]" = field(init=False, default=None)
def __post_init__(self):
assert self.grid_map is not None, "grid_map must be provided"
object.__setattr__(
self, "_all_centers", tuple(carla.Location(x, y, self.z) for x, y in self.grid_map.all_centers)
)
if self.draw_pos:
object.__setattr__(self, "_all_pos", tuple(carla.Location(x, y, self.z) for x, y in self.grid_map.all_pos))
# Account for the rotation of the grid map in the extent
rotated_width = (
self.grid_map.cell_width / 2 * self.grid_map.rotation_cos
+ self.grid_map.cell_height / 2 * self.grid_map.rotation_sin
)
rotated_height = (
self.grid_map.cell_height / 2 * self.grid_map.rotation_cos
+ self.grid_map.cell_width / 2 * self.grid_map.rotation_sin
)
object.__setattr__(self, "_extent", carla.Vector3D(rotated_width, rotated_height))
# Compute corners
corner1 = carla.Location(*self.grid_map.cell_to_pos((0, 0)))
corner2 = carla.Location(*self.grid_map.cell_to_pos((0, self.grid_map.grid.shape[1] - 1)))
corner3 = carla.Location(
*self.grid_map.cell_to_pos((self.grid_map.grid.shape[0] - 1, self.grid_map.grid.shape[1] - 1))
)
corner4 = carla.Location(*self.grid_map.cell_to_pos((self.grid_map.grid.shape[0] - 1, 0)))
object.__setattr__(self, "_corners", (corner1, corner2, corner3, corner4))
[docs]
def draw(self, world: "carla.World"):
for corner in self._corners:
world.debug.draw_point(
corner,
size=0.1,
life_time=self.life_time,
color=self.color,
)
for center in self._all_centers:
world.debug.draw_box(
carla.BoundingBox(center, self._extent),
rotation=self._rotation,
thickness=self.thickness,
life_time=self.life_time,
color=self.color,
)
for pos in self._all_pos:
world.debug.draw_point(
pos,
size=0.1,
life_time=self.life_time,
color=carla.Color(0, 255, 0),
)
r, c = self.grid_map.pos_to_cell((pos.x, pos.y))
world.debug.draw_string(
pos, f"({pos.x:.2f}, {pos.y:.2f})", draw_shadow=False, color=carla.Color(), life_time=600
)
world.debug.draw_string(
pos + carla.Location(0, 1), f"({r}, {c})", draw_shadow=False, color=carla.Color(), life_time=600
)
world.debug.draw_point(location=pos, size=0.1, color=carla.Color(255), life_time=self.life_time)
[docs]
@dataclass(frozen=True)
class WaypointGizmo(Gizmo):
"""A gizmo that draws waypoint grid.
Displays a grid of waypoints with the specified origin, shape, and size.
"""
origin: "carla.Location" = field(default_factory=carla.Location)
shape: "tuple[int, int]" = field(default=(1, 1))
size: "tuple[float, float]" = field(default=(1.0, 1.0))
rotation: "carla.Rotation" = field(default=carla.Rotation)
thickness: float = field(default=0.06)
def __post_init__(self):
if isinstance(self.origin, (np.ndarray, list, tuple)):
object.__setattr__(self, "origin", carla.Location(*self.origin))
[docs]
def draw(self, world: "carla.World"):
ox, oy, oz = self.origin.x, self.origin.y, self.origin.z
extent = carla.Vector3D(self.size[0] / 2, self.size[1] / 2, 0)
for x, y in product(range(self.shape[0]), range(self.shape[1])):
corner = carla.Location(ox + extent.x + x * self.size[0], oy + extent.y + y * self.size[1], oz)
world.debug.draw_point(
corner,
rotation=self.rotation,
thickness=self.thickness,
life_time=self.life_time,
color=self.color,
)
# corner.x -= self.cell_size[0] / 2
# corner.y -= self.cell_size[1] / 2
# world.debug.draw_string(
# corner, f"({corner.x}, {corner.y})", draw_shadow=True, color=carla.Color(), life_time=1.0
# )
[docs]
@dataclass(frozen=True)
class PathGizmo(Gizmo):
"""A gizmo that draws a path/trajectory.
Displays a series of waypoints connected as a path with directional arrows
and optional labels at each waypoint.
"""
waypoints: "list[carla.Location | carla.Waypoint]" = field(default_factory=list)
size: float = field(default=0.2)
z: float = field(default=0.0)
label: str = ""
def __post_init__(self):
waypoints = [
(
carla.Location(*point)
if isinstance(point, (np.ndarray, list, tuple))
else (point.transform.location if isinstance(point, carla.Waypoint) else point)
)
for point in self.waypoints
]
object.__setattr__(self, "waypoints", waypoints)
[docs]
def draw(self, world: "carla.World"):
for i, point in enumerate(self.waypoints):
# If this is not the first point, compute the rotation from the previous point to this point
if i > 0:
dx = point.x - self.waypoints[i - 1].x
dy = point.y - self.waypoints[i - 1].y
rotation = carla.Rotation(0, np.degrees(np.arctan2(dy, dx)), 0)
else:
rotation = carla.Rotation()
world.debug.draw_box(
carla.BoundingBox(point, carla.Vector3D(self.size, self.size)),
rotation=rotation,
thickness=self.thickness,
life_time=self.life_time,
color=self.color,
)
if self.label:
world.debug.draw_string(
point + carla.Location(0.1, 0.1, 0.5),
self.label,
draw_shadow=False,
color=carla.Color(),
life_time=600,
)