from dataclasses import dataclass, field
from typing import Callable
import carla
import numpy as np
from symaware.base import Entity as BaseEntity
from symaware.base import NullDynamicalModel
from .dynamical_model import DynamicalModel
[docs]
@dataclass(frozen=True)
class Entity(BaseEntity):
"""Abstract class for CARLA entities in the simulation.
Represents an actor in the CARLA simulation environment. All internal identifiers
are set to -1 by default and will be set to the correct values during initialisation.
Args
----
position:
(3)-shaped initial position of the entity. If the size is less than 3, missing values are set to 0
velocity:
(3)-shaped initial velocity of the entity. If the size is less than 3, missing values are set to 0
rotation:
(3)-shaped initial orientation of the entity as Euler angles
model:
Dynamical model associated with the entity. Must be a subclass of `DynamicalModel`
"""
id: int = field(default=-1)
actor_id: int = field(default=-1)
kind: str = field(default="")
actor: "carla.Actor" = field(default=None)
position: np.ndarray = field(default_factory=lambda: np.zeros(3))
velocity: np.ndarray = field(default_factory=lambda: np.zeros(3))
rotation: np.ndarray = field(default_factory=lambda: np.zeros(3))
acceleration: np.ndarray = field(default_factory=lambda: np.zeros(3))
model: "DynamicalModel" = field(default_factory=NullDynamicalModel)
pre_existing: bool = field(default=False)
kwargs: "dict[str, str]" = field(default_factory=dict)
@property
def state(self):
"""
Get the state of the entity as a numpy array.
The state is defined as the position and orientation of the entity.
Returns
-------
(3 + 3 + 3)-shaped array containing the position, orientation and velocity of the entity.
"""
transform, velocity = self.actor.get_transform(), self.actor.get_velocity()
return np.array(
[
transform.location.x,
transform.location.y,
transform.location.z,
transform.rotation.roll,
transform.rotation.pitch,
transform.rotation.yaw,
velocity.x,
velocity.y,
velocity.z,
]
)
@property
def direction(self) -> "carla.Vector3D":
"""
Get the direction of the entity as a numpy array.
The direction is defined as the orientation of the entity.
Returns
-------
Vector representing the direction the entity is facing.
"""
assert isinstance(self.actor, carla.Vehicle), "Actor must be a carla.Actor instance"
return self.actor.get_physics_control().wheels[0].position - self.actor.get_physics_control().wheels[2].position
@property
def location(self) -> "carla.Location":
"""
Get the location of the entity as a carla.Location object.
Returns
-------
Location of the entity
"""
return self.actor.get_transform().location if self.actor else carla.Location(*self.position[:3])
def __post_init__(self):
object.__setattr__(
self, "position", np.pad(self.position[:3], (0, max(0, 3 - len(self.position))), mode="constant")
)
object.__setattr__(
self, "velocity", np.pad(self.velocity[:3], (0, max(0, 3 - len(self.velocity))), mode="constant")
)
object.__setattr__(
self,
"acceleration",
np.pad(self.acceleration[:3], (0, max(0, 3 - len(self.acceleration))), mode="constant"),
)
if len(self.rotation) == 3:
object.__setattr__(self, "orientation", np.array(self.rotation))
else:
raise ValueError(f"Invalid orientation {self.rotation}")
[docs]
def initialise(self, world: "carla.World", parent: "carla.Actor" = None):
"""
Initialise the entity in the world.
This method should be called after the entity is created.
It sets the actor_id of the entity and the actor associated with it.
Args
----
world:
The world in which the entity is created.
"""
if self.actor is not None:
object.__setattr__(self, "actor_id", self.actor.id)
object.__setattr__(self, "pre_existing", True)
if self.actor is None and self.actor_id != -1:
object.__setattr__(self, "actor", world.get_actor(self.actor_id))
object.__setattr__(self, "pre_existing", True)
if self.actor is None:
object.__setattr__(self, "actor", self.spawn_actor(world, parent, **self.kwargs))
object.__setattr__(self, "actor_id", self.actor.id)
if isinstance(self.model, DynamicalModel):
self.model.initialise(self.actor)
[docs]
def spawn_actor(self, world: "carla.World", parent: "carla.Actor" = None, **kwargs) -> "carla.Actor":
actor_bp = world.get_blueprint_library().find(self.kind)
for key, value in kwargs.items():
actor_bp.set_attribute(key, value)
try:
actor: "carla.Actor" = world.spawn_actor(
actor_bp,
carla.Transform(location=carla.Location(*self.position), rotation=carla.Rotation(*self.rotation)),
attach_to=parent,
)
except RuntimeError as e:
print(f"Failed to spawn actor '{type}' at {self.position} with rotation {self.rotation}")
raise e
return actor
def __hash__(self) -> int:
return hash((super().__hash__(), self.actor.id if self.actor else -1))
[docs]
@dataclass(frozen=True)
class SensorEntity(Entity):
"""Abstract base class for sensor entities in CARLA.
Represents sensors (cameras, lidar, etc.) in the CARLA simulation.
All internal identifiers are set to -1 by default and will be set to the correct values during initialisation.
Args
----
position:
(3)-shaped initial position of the entity. If the size is less than 3, missing values are set to 0
velocity:
(3)-shaped initial velocity of the entity. If the size is less than 3, missing values are set to 0
rotation:
(3)-shaped initial orientation of the entity as Euler angles
model:
Dynamical model associated with the entity. Must be a subclass of `DynamicalModel`
"""
actor: "carla.Sensor" = field(default=None)
cbs: "list[callable]" = field(default_factory=list)
[docs]
def spawn_actor(self, world: "carla.World", parent: "carla.Actor" = None, **kwargs) -> "carla.Sensor":
sensor: "carla.Sensor" = super().spawn_actor(world, parent, **kwargs)
for cb in self.cbs:
sensor.listen(cb)
return sensor
[docs]
def stop(self):
if self.actor is not None:
self.actor.stop()
self.actor.destroy()
def __hash__(self) -> int:
return super().__hash__()
[docs]
@dataclass(frozen=True)
class CameraEntity(SensorEntity):
"""Camera sensor entity for CARLA simulation.
Represents a camera sensor that captures RGB images from the simulation.
All internal identifiers are set to -1 by default and will be set to the correct values during initialisation.
Args
----
position:
(3)-shaped initial position of the entity. If the size is less than 3, missing values are set to 0
velocity:
(3)-shaped initial velocity of the entity. If the size is less than 3, missing values are set to 0
rotation:
(3)-shaped initial orientation of the entity as Euler angles
model:
Dynamical model associated with the entity. Must be a subclass of `DynamicalModel`
"""
actor: "carla.Sensor" = field(default=None)
kind: str = field(default="sensor.camera.rgb")
callback: "Callable[[carla.Image], None] | None" = None
[docs]
def spawn_actor(self, world: "carla.World", parent: "carla.Actor" = None, **kwargs) -> "carla.Actor":
actor = super().spawn_actor(world, parent, **kwargs)
if self.callback is not None:
actor.listen(self.callback)
return actor
def __hash__(self) -> int:
return super().__hash__()
[docs]
@dataclass(frozen=True)
class SpectatorEntity(Entity):
"""Spectator camera entity for CARLA simulation.
Provides a special viewport to view the simulation from a detached perspective.
All internal identifiers are set to -1 by default and will be set to the correct values during initialisation.
Args
----
position:
(3)-shaped initial position of the entity. If the size is less than 3, missing values are set to 0
velocity:
(3)-shaped initial velocity of the entity. If the size is less than 3, missing values are set to 0
rotation:
(3)-shaped initial orientation of the entity as Euler angles
model:
Dynamical model associated with the entity. Must be a subclass of `DynamicalModel`
"""
parent: "carla.Actor" = field(default=None)
following: bool = field(default=False)
[docs]
def initialise(self, world, parent=None):
spectator = world.get_spectator()
object.__setattr__(self, "actor_id", spectator.id)
object.__setattr__(self, "actor", spectator)
object.__setattr__(self, "parent", parent)
object.__setattr__(self, "pre_existing", True)
if self.parent is None:
self.actor.set_transform(carla.Transform(carla.Location(*self.position), carla.Rotation(*self.rotation)))
else:
self.actor.set_transform(
carla.Transform(
self.parent.get_transform().transform(carla.Location(*self.position)),
carla.Rotation(*self.rotation),
)
)
[docs]
def spawn_actor(self, world: "carla.World", parent: "carla.Actor" = None, **kwargs) -> "carla.Sensor":
raise NotImplementedError("SpectatorEntity cannot be spawned as an actor")
[docs]
def step(self):
if self.parent is None or not self.following:
return
self.actor.set_transform(
carla.Transform(
self.parent.get_transform().transform(carla.Location(*self.position)), carla.Rotation(*self.rotation)
)
)
def __hash__(self) -> int:
return super().__hash__()
[docs]
@dataclass(frozen=True)
class VehicleEntity(Entity):
"""Vehicle entity for CARLA simulation.
Represents a vehicle actor in the CARLA simulation environment.
All internal identifiers are set to -1 by default and will be set to the correct values during initialisation.
Args
----
position:
(3)-shaped initial position of the entity. If the size is less than 3, missing values are set to 0
velocity:
(3)-shaped initial velocity of the entity. If the size is less than 3, missing values are set to 0
rotation:
(3)-shaped initial orientation of the entity as Euler angles
model:
Dynamical model associated with the entity. Must be a subclass of `DynamicalModel`
"""
actor: "carla.Vehicle" = field(default=None)
color: str = field(default="255,0,0")
kind: str = field(default="vehicle.tesla.model3")
autopilot: bool = field(default=False)
gravity: bool = field(default=False)
sensors: "list[Entity]" = field(default_factory=list)
wheel_base: float = 0.0
[docs]
def initialise(self, world, parent=None):
super().initialise(world, parent)
wb_vec: "carla.Vector3D" = (
self.actor.get_physics_control().wheels[0].position - self.actor.get_physics_control().wheels[2].position
)
wheel_base = np.sqrt(wb_vec.x**2 + wb_vec.y**2 + wb_vec.z**2) / 100
object.__setattr__(self, "wheel_base", wheel_base)
@property
def state(self):
"""
Get the state of the entity as a numpy array.
The state is defined as the position and orientation of the entity.
Returns
-------
np.ndarray
(3 + 3 + 3)-shaped array containing the position (m), orientation (rad) and velocity (m/s**2) of the entity.
"""
assert isinstance(self.actor, carla.Vehicle), "Actor must be a carla.Actor instance"
transform, velocity = self.actor.get_transform(), self.actor.get_velocity()
# wheel_base = (
# self.actor.get_physics_control().wheels[0].position - self.actor.get_physics_control().wheels[2].position
# )
# yaw = math.atan2(wheel_base.y, wheel_base.x)
# assert np.isclose(
# np.rad2deg(yaw), transform.rotation.yaw
# ), f"Yaw mismatch between wheel base and transform: {np.rad2deg(yaw)} vs {transform.rotation.yaw}"
return np.array(
[
transform.location.x,
transform.location.y,
transform.location.z,
np.deg2rad(transform.rotation.roll),
np.deg2rad(transform.rotation.pitch),
np.deg2rad(transform.rotation.yaw),
velocity.x,
velocity.y,
velocity.z,
]
)
[docs]
def spawn_actor(self, world: "carla.World", parent: "carla.Actor" = None, **kwargs) -> "carla.Vehicle":
vehicle: "carla.Vehicle" = super().spawn_actor(world, parent, color=self.color)
vehicle.set_autopilot(self.autopilot)
vehicle.set_enable_gravity(self.gravity)
for sensor in self.sensors:
sensor.initialise(world, parent=vehicle)
return vehicle
[docs]
def step(self):
super().step()
for sensor in self.sensors:
sensor.step()
[docs]
def stop(self):
for sensor in self.sensors:
sensor.stop()
def __hash__(self) -> int:
return super().__hash__()
[docs]
@dataclass(frozen=True)
class PedestrianEntity(Entity):
"""Pedestrian entity for CARLA simulation.
Represents a pedestrian (walker) actor in the CARLA simulation environment.
All internal identifiers are set to -1 by default and will be set to the correct values during initialisation.
Args
----
position:
(3)-shaped initial position of the entity. If the size is less than 3, missing values are set to 0
velocity:
(3)-shaped initial velocity of the entity. If the size is less than 3, missing values are set to 0
rotation:
(3)-shaped initial orientation of the entity as Euler angles
model:
Dynamical model associated with the entity. Must be a subclass of `DynamicalModel`
"""
kind: str = field(default="walker.pedestrian.0038")
def __hash__(self) -> int:
return super().__hash__()
[docs]
@dataclass(frozen=True)
class WarningSignEntity(Entity):
"""Warning sign entity for CARLA simulation.
Represents a warning sign or static object in the CARLA simulation environment.
All internal identifiers are set to -1 by default and will be set to the correct values during initialisation.
Args
----
position:
(3)-shaped initial position of the entity. If the size is less than 3, missing values are set to 0
velocity:
(3)-shaped initial velocity of the entity. If the size is less than 3, missing values are set to 0
rotation:
(3)-shaped initial orientation of the entity as Euler angles
model:
Dynamical model associated with the entity. Must be a subclass of `DynamicalModel`
"""
kind: str = field(default="static.prop.trafficwarning")
def __hash__(self) -> int:
return super().__hash__()