From 7239f5623f32745e78a7b7ab987cad613f27ed5c Mon Sep 17 00:00:00 2001 From: Mirco Theile Date: Thu, 7 Sep 2023 11:49:45 -0700 Subject: [PATCH 1/6] Added initial sensors implementation --- .../data_generation/data_generation.scenic | 30 +++++ src/scenic/core/object_types.py | 23 +++- src/scenic/core/sensors.py | 30 +++++ src/scenic/core/simulators.py | 6 + src/scenic/domains/driving/model.scenic | 13 +++ src/scenic/simulators/carla/model.scenic | 4 + src/scenic/simulators/carla/sensors.py | 109 ++++++++++++++++++ src/scenic/simulators/carla/simulator.py | 54 ++++++--- 8 files changed, 251 insertions(+), 18 deletions(-) create mode 100644 examples/sensors/carla/data_generation/data_generation.scenic create mode 100644 src/scenic/core/sensors.py create mode 100644 src/scenic/simulators/carla/sensors.py diff --git a/examples/sensors/carla/data_generation/data_generation.scenic b/examples/sensors/carla/data_generation/data_generation.scenic new file mode 100644 index 000000000..f8555d0e8 --- /dev/null +++ b/examples/sensors/carla/data_generation/data_generation.scenic @@ -0,0 +1,30 @@ +param map = localPath('../../../../assets/maps/CARLA/Town05.xodr') +param carla_map = 'Town05' +model scenic.simulators.carla.model + +# Sample a lane at random +lane = Uniform(*network.lanes) + +spot = new OrientedPoint on lane.centerline + +attrs = {"image_size_x": 1056, + "image_size_y": 704} + +car_model = "vehicle.tesla.model3" + +# Spawn car on that spot with logging autopilot behavior and +# - an RGB Camera pointing forward with specific attributes +# - a semantic segmentation sensor +ego = new Car at spot, + with blueprint car_model, + with behavior AutopilotBehavior(), + with sensors {"front_ss": SSSensor(offset=(1.6, 0, 1.7), convert='CityScapesPalette', attributes=attrs), + "front_rgb": RGBSensor(offset=(1.6, 0, 1.7), attributes=attrs)} + +other = new Car offset by 0 @ Range(10, 30), + with behavior AutopilotBehavior() + +#record ego.observations["front_rgb"] as "front_rgb" after 5s every 1s # TODO Implement +#record ego.observations["front_rgb"] after 5s every 1s to localPath("data/generation") # TODO Implement + +require monitor RecordingMonitor(ego, path=localPath("data"), recording_start=5, subsample=5) diff --git a/src/scenic/core/object_types.py b/src/scenic/core/object_types.py index cd4ee9961..a3b0f6829 100644 --- a/src/scenic/core/object_types.py +++ b/src/scenic/core/object_types.py @@ -967,6 +967,9 @@ class Object(OrientedPoint): value ``None``. lastActions: Tuple of :term:`actions` taken by this agent in the last time step (or `None` if the object is not an agent or this is the first time step). + sensors: Dict of ("name": sensor) that populate the observations field every time step + observations: Dict of ("name": observation) storing the latest observation of the sensor + with the same name """ _scenic_properties = { @@ -994,6 +997,9 @@ class Object(OrientedPoint): "lastActions": None, # weakref to scenario which created this object, for internal use "_parentScenario": None, + # Sensor properties + "sensors": {}, + "observations": {} } def __new__(cls, *args, **kwargs): @@ -1430,7 +1436,7 @@ def show3D(self, viewer, highlight=False): edges = view_region_mesh.face_adjacency_edges[ view_region_mesh.face_adjacency_angles > np.radians(0.1) - ] + ] vertices = view_region_mesh.vertices edge_path = trimesh.path.Path3D( @@ -1482,9 +1488,9 @@ def show2D(self, workspace, plt, highlight=False): def _isPlanarBox(self): """Whether this object is a box aligned with the XY plane.""" return ( - isinstance(self.shape, BoxShape) - and self.orientation.pitch == 0 - and self.orientation.roll == 0 + isinstance(self.shape, BoxShape) + and self.orientation.pitch == 0 + and self.orientation.roll == 0 ) @cached_property @@ -1502,11 +1508,18 @@ def _boundingPolygon(self): length * cyaw, pos[0], pos[1], - ] + ] return shapely.affinity.affine_transform(_unitBox, matrix) return self.occupiedSpace._boundingPolygon + def save_observations(self, save_path, frame_number): + import os + for key, sensor in self.sensors.items(): + sensor_path = os.path.join(save_path, key) + os.makedirs(sensor_path, exist_ok=True) + sensor.save_last_observation(save_path=sensor_path, frame_number=frame_number) + _unitBox = shapely.geometry.Polygon(((0.5, 0.5), (-0.5, 0.5), (-0.5, -0.5), (0.5, -0.5))) diff --git a/src/scenic/core/sensors.py b/src/scenic/core/sensors.py new file mode 100644 index 000000000..48bc87e52 --- /dev/null +++ b/src/scenic/core/sensors.py @@ -0,0 +1,30 @@ +import copy + + +class Sensor: + def get_observation(self): + raise NotImplementedError("Not implemented for passive sensors") + + def save_last_observation(self, save_path, frame_number=None): + raise NotImplementedError("Implement in subclass") + + +class ActiveSensor(Sensor): + + def __init__(self): + self.observation = None + self.frame = 0 + + def get_observation(self): + return self.observation + + def on_data(self, data): + print("On data", self.__class__) + print("Frame", data.frame) + self.observation, self.frame = self.processing(data) + + def processing(self, data): + raise NotImplementedError("Implement in subclass") + + def save_last_observation(self, save_path, frame_number=None): + raise NotImplementedError("Implement in subclass") diff --git a/src/scenic/core/simulators.py b/src/scenic/core/simulators.py index 04e74efd1..4a67491db 100644 --- a/src/scenic/core/simulators.py +++ b/src/scenic/core/simulators.py @@ -426,6 +426,12 @@ def _run(self, dynamicScenario, maxSteps): terminationReason = dynamicScenario._step() terminationType = TerminationType.scenarioComplete + # Update observations of objects with sensors + for obj in self.objects: + if not obj.sensors: + continue + obj.observations.update({key: sensor.get_observation() for key, sensor in obj.sensors.items()}) + # Record current state of the simulation self.recordCurrentState() diff --git a/src/scenic/domains/driving/model.scenic b/src/scenic/domains/driving/model.scenic index 0dea752f2..8fdc9c73a 100644 --- a/src/scenic/domains/driving/model.scenic +++ b/src/scenic/domains/driving/model.scenic @@ -349,3 +349,16 @@ def withinDistanceToObjsInLane(vehicle, thresholdDistance): if (distance from vehicle to obj) < thresholdDistance: return True return False + + +monitor RecordingMonitor(car, path, recording_start=1, subsample=1): + # Exclude the first recording_start steps, e.g., because cars are spawned in the air and need to drop down first + for _ in range(1, recording_start): + wait + + frame_number = 1 + while True: + car.save_observations(path, frame_number=frame_number) + frame_number += 1 + for _ in range(subsample): + wait diff --git a/src/scenic/simulators/carla/model.scenic b/src/scenic/simulators/carla/model.scenic index 6a9fa954d..36cdaf98c 100644 --- a/src/scenic/simulators/carla/model.scenic +++ b/src/scenic/simulators/carla/model.scenic @@ -46,6 +46,10 @@ import scenic.simulators.carla.blueprints as blueprints from scenic.simulators.carla.behaviors import * from scenic.simulators.utils.colors import Color +# Sensor imports +from scenic.simulators.carla.sensors import CarlaRGBSensor as RGBSensor +from scenic.simulators.carla.sensors import CarlaSSSensor as SSSensor + try: from scenic.simulators.carla.simulator import CarlaSimulator # for use in scenarios from scenic.simulators.carla.actions import * diff --git a/src/scenic/simulators/carla/sensors.py b/src/scenic/simulators/carla/sensors.py new file mode 100644 index 000000000..8ce925f1b --- /dev/null +++ b/src/scenic/simulators/carla/sensors.py @@ -0,0 +1,109 @@ +from time import sleep + +import carla +import numpy as np + +from scenic.core.sensors import ActiveSensor +import os +import cv2 + +class CarlaVisionSensor(ActiveSensor): + + def __init__(self, offset=(0, 0, 0), rotation=(0, 0, 0), attributes=None, + blueprint='sensor.camera.rgb', convert=None, record_npy=False): + super().__init__() + self.transform = carla.Transform(carla.Location(x=offset[0], y=offset[1], z=offset[2]), + carla.Rotation(pitch=rotation[0], yaw=rotation[1], roll=rotation[2])) + self.blueprint = blueprint + if isinstance(attributes, str): + raise NotImplementedError("String parsing for attributes is not yet implemented. Feel free to do so.") + elif isinstance(attributes, dict): + self.attributes = attributes + else: + self.attributes = {} + + self.convert = None + if convert is not None: + if isinstance(convert, int): + self.convert = convert + elif isinstance(convert, str): + self.convert = carla.ColorConverter.names[convert] + else: + AttributeError("'convert' has to be int or string.") + + self.record_npy = record_npy + + # def processing(self, data): + # if self.convert is not None: + # data.convert(self.convert) + # return data + # + # array = np.frombuffer(data.raw_data, dtype=np.dtype("uint8")) + # array = np.reshape(array, (data.height, data.width, 4)) # BGRA format + # array = array[:, :, :3] # Take only RGB + # array = array[:, :, ::-1] # Revert order + # + # return array + + def save_last_observation(self, save_path, frame_number=None): + raise NotImplementedError() + + +class CarlaRGBSensor(CarlaVisionSensor): + def __init__(self, offset=(0, 0, 0), rotation=(0, 0, 0), attributes=None, + record_npy=False): + super().__init__(offset=offset, rotation=rotation, attributes=attributes, + blueprint='sensor.camera.rgb', convert=None, record_npy=record_npy) + + def processing(self, data): + + array = np.frombuffer(data.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (data.height, data.width, 4)) # BGRA format + array = array[:, :, :3] # Take only RGB + array = array[:, :, ::-1] # Revert order + + return array, data.frame + + def save_last_observation(self, save_path, frame_number=None): + if frame_number is None: + frame_number = self.frame + save_as = os.path.join(save_path, f"{frame_number}") + if self.record_npy: + np.save(save_as, self.observation) + else: + cv2.imwrite(f"{save_as}.png", self.observation[..., ::-1]) + + +class CarlaSSSensor(CarlaVisionSensor): + def __init__(self, offset=(0, 0, 0), rotation=(0, 0, 0), attributes=None, convert=None, + record_npy=False): + super().__init__(offset=offset, rotation=rotation, attributes=attributes, + blueprint='sensor.camera.semantic_segmentation', convert=convert, + record_npy=record_npy) + + def processing(self, data): + if self.convert is not None: + data.convert(self.convert) + + array = np.frombuffer(data.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (data.height, data.width, 4)) # BGRA format + + if self.convert is not None: + array = array[:, :, :3] # Take only RGB + array = array[:, :, ::-1] # Revert order + else: + array = array[:, :, 2] # Take only R + + return array, data.frame + + def save_last_observation(self, save_path, frame_number=None): + if frame_number is None: + frame_number = self.observation.frame + save_as = os.path.join(save_path, f"{frame_number}") + if self.record_npy: + np.save(save_as, self.observation) + else: + if self.convert is not None: + cv2.imwrite(f"{save_as}.png", self.observation[..., ::-1]) + else: + cv2.imwrite(f"{save_as}.png", self.observation) diff --git a/src/scenic/simulators/carla/simulator.py b/src/scenic/simulators/carla/simulator.py index 44cc05b36..d37f91c5b 100644 --- a/src/scenic/simulators/carla/simulator.py +++ b/src/scenic/simulators/carla/simulator.py @@ -1,4 +1,5 @@ """Simulator interface for CARLA.""" +from time import sleep try: import carla @@ -28,16 +29,16 @@ class CarlaSimulator(DrivingSimulator): """Implementation of `Simulator` for CARLA.""" def __init__( - self, - carla_map, - map_path, - address="127.0.0.1", - port=2000, - timeout=10, - render=True, - record="", - timestep=0.1, - traffic_manager_port=None, + self, + carla_map, + map_path, + address="127.0.0.1", + port=2000, + timeout=10, + render=True, + record="", + timestep=0.1, + traffic_manager_port=None, ): super().__init__() verbosePrint(f"Connecting to CARLA on port {port}") @@ -104,6 +105,7 @@ class CarlaSimulation(DrivingSimulation): def __init__(self, scene, client, tm, render, record, scenario_number, **kwargs): self.client = client self.world = self.client.get_world() + self.current_frame = None self.map = self.world.get_map() self.blueprintLib = self.world.get_blueprint_library() self.tm = tm @@ -162,7 +164,7 @@ def setup(self): carla.VehicleControl(manual_gear_shift=False) ) - self.world.tick() + self.current_frame = self.world.tick() for obj in self.objects: if obj.speed is not None and obj.speed != 0: @@ -211,7 +213,7 @@ def createObjectInSimulator(self, obj): # Color, cannot be set for Pedestrians if blueprint.has_attribute("color") and obj.color is not None: c = obj.color - c_str = f"{int(c.r*255)},{int(c.g*255)},{int(c.b*255)}" + c_str = f"{int(c.r * 255)},{int(c.g * 255)},{int(c.b * 255)}" blueprint.set_attribute("color", c_str) # Create Carla actor @@ -243,6 +245,20 @@ def createObjectInSimulator(self, obj): f"Unable to spawn carla controller for object {obj}" ) obj.carlaController = controller + + # Adding sensors if available + if obj.sensors: + for sensor_key, sensor in obj.sensors.items(): + sensor_bp = self.blueprintLib.find(sensor.blueprint) + for key, val in sensor.attributes.items(): + if sensor_bp.has_attribute(key): + sensor_bp.set_attribute(key, str(val)) + + carla_sensor = self.world.spawn_actor(sensor_bp, sensor.transform, attach_to=obj.carlaActor) + carla_sensor.listen(sensor.on_data) + sensor.carla_sensor = carla_sensor + obj.observations = {} + return carlaActor def executeActions(self, allActions): @@ -257,7 +273,14 @@ def executeActions(self, allActions): def step(self): # Run simulation for one timestep - self.world.tick() + self.current_frame = self.world.tick() + + # Wait for sensors to get updates + for obj in self.objects: + if obj.sensors: + for sensor in obj.sensors.values(): + while sensor.frame != self.current_frame: + pass # Render simulation if self.render: @@ -298,6 +321,11 @@ def getProperties(self, obj, properties): def destroy(self): for obj in self.objects: + if obj.sensors: + for sensor in obj.sensors.values(): + if sensor.carla_sensor is not None and sensor.carla_sensor.is_alive: + sensor.carla_sensor.stop() + sensor.carla_sensor.destroy() if obj.carlaActor is not None: if isinstance(obj.carlaActor, carla.Vehicle): obj.carlaActor.set_autopilot(False, self.tm.get_port()) From 11a7727e123f5ea61d31222951edcdfb475ecd00 Mon Sep 17 00:00:00 2001 From: Mirco Theile Date: Wed, 11 Oct 2023 18:57:45 +0200 Subject: [PATCH 2/6] Minor fixes --- .../carla/data_generation/data_generation.scenic | 5 ++++- src/scenic/simulators/carla/sensors.py | 12 ------------ src/scenic/simulators/carla/simulator.py | 1 - 3 files changed, 4 insertions(+), 14 deletions(-) diff --git a/examples/sensors/carla/data_generation/data_generation.scenic b/examples/sensors/carla/data_generation/data_generation.scenic index f8555d0e8..7a3d5740b 100644 --- a/examples/sensors/carla/data_generation/data_generation.scenic +++ b/examples/sensors/carla/data_generation/data_generation.scenic @@ -19,11 +19,14 @@ ego = new Car at spot, with blueprint car_model, with behavior AutopilotBehavior(), with sensors {"front_ss": SSSensor(offset=(1.6, 0, 1.7), convert='CityScapesPalette', attributes=attrs), - "front_rgb": RGBSensor(offset=(1.6, 0, 1.7), attributes=attrs)} + "front_rgb": RGBSensor(offset=(1.6, 0, 1.7), attributes=attrs) + } + other = new Car offset by 0 @ Range(10, 30), with behavior AutopilotBehavior() +record ego.observations["front_rgb"] as "front_rgb" #record ego.observations["front_rgb"] as "front_rgb" after 5s every 1s # TODO Implement #record ego.observations["front_rgb"] after 5s every 1s to localPath("data/generation") # TODO Implement diff --git a/src/scenic/simulators/carla/sensors.py b/src/scenic/simulators/carla/sensors.py index 8ce925f1b..651313533 100644 --- a/src/scenic/simulators/carla/sensors.py +++ b/src/scenic/simulators/carla/sensors.py @@ -33,18 +33,6 @@ def __init__(self, offset=(0, 0, 0), rotation=(0, 0, 0), attributes=None, self.record_npy = record_npy - # def processing(self, data): - # if self.convert is not None: - # data.convert(self.convert) - # return data - # - # array = np.frombuffer(data.raw_data, dtype=np.dtype("uint8")) - # array = np.reshape(array, (data.height, data.width, 4)) # BGRA format - # array = array[:, :, :3] # Take only RGB - # array = array[:, :, ::-1] # Revert order - # - # return array - def save_last_observation(self, save_path, frame_number=None): raise NotImplementedError() diff --git a/src/scenic/simulators/carla/simulator.py b/src/scenic/simulators/carla/simulator.py index d37f91c5b..813e5042e 100644 --- a/src/scenic/simulators/carla/simulator.py +++ b/src/scenic/simulators/carla/simulator.py @@ -1,5 +1,4 @@ """Simulator interface for CARLA.""" -from time import sleep try: import carla From e145be1316c5ee4e8c035be27e9a638b523b8d77 Mon Sep 17 00:00:00 2001 From: Mirco Theile Date: Tue, 14 Nov 2023 09:22:00 +0100 Subject: [PATCH 3/6] Formatting --- src/scenic/core/object_types.py | 13 ++--- src/scenic/core/sensors.py | 1 - src/scenic/core/simulators.py | 4 +- src/scenic/simulators/carla/sensors.py | 64 +++++++++++++++++------- src/scenic/simulators/carla/simulator.py | 24 +++++---- 5 files changed, 69 insertions(+), 37 deletions(-) diff --git a/src/scenic/core/object_types.py b/src/scenic/core/object_types.py index a3b0f6829..15fcdaa1a 100644 --- a/src/scenic/core/object_types.py +++ b/src/scenic/core/object_types.py @@ -999,7 +999,7 @@ class Object(OrientedPoint): "_parentScenario": None, # Sensor properties "sensors": {}, - "observations": {} + "observations": {}, } def __new__(cls, *args, **kwargs): @@ -1436,7 +1436,7 @@ def show3D(self, viewer, highlight=False): edges = view_region_mesh.face_adjacency_edges[ view_region_mesh.face_adjacency_angles > np.radians(0.1) - ] + ] vertices = view_region_mesh.vertices edge_path = trimesh.path.Path3D( @@ -1488,9 +1488,9 @@ def show2D(self, workspace, plt, highlight=False): def _isPlanarBox(self): """Whether this object is a box aligned with the XY plane.""" return ( - isinstance(self.shape, BoxShape) - and self.orientation.pitch == 0 - and self.orientation.roll == 0 + isinstance(self.shape, BoxShape) + and self.orientation.pitch == 0 + and self.orientation.roll == 0 ) @cached_property @@ -1508,13 +1508,14 @@ def _boundingPolygon(self): length * cyaw, pos[0], pos[1], - ] + ] return shapely.affinity.affine_transform(_unitBox, matrix) return self.occupiedSpace._boundingPolygon def save_observations(self, save_path, frame_number): import os + for key, sensor in self.sensors.items(): sensor_path = os.path.join(save_path, key) os.makedirs(sensor_path, exist_ok=True) diff --git a/src/scenic/core/sensors.py b/src/scenic/core/sensors.py index 48bc87e52..f6514a9fe 100644 --- a/src/scenic/core/sensors.py +++ b/src/scenic/core/sensors.py @@ -10,7 +10,6 @@ def save_last_observation(self, save_path, frame_number=None): class ActiveSensor(Sensor): - def __init__(self): self.observation = None self.frame = 0 diff --git a/src/scenic/core/simulators.py b/src/scenic/core/simulators.py index 4a67491db..8d1340930 100644 --- a/src/scenic/core/simulators.py +++ b/src/scenic/core/simulators.py @@ -430,7 +430,9 @@ def _run(self, dynamicScenario, maxSteps): for obj in self.objects: if not obj.sensors: continue - obj.observations.update({key: sensor.get_observation() for key, sensor in obj.sensors.items()}) + obj.observations.update( + {key: sensor.get_observation() for key, sensor in obj.sensors.items()} + ) # Record current state of the simulation self.recordCurrentState() diff --git a/src/scenic/simulators/carla/sensors.py b/src/scenic/simulators/carla/sensors.py index 8ce925f1b..efe3835e0 100644 --- a/src/scenic/simulators/carla/sensors.py +++ b/src/scenic/simulators/carla/sensors.py @@ -1,22 +1,33 @@ +import os from time import sleep import carla +import cv2 import numpy as np from scenic.core.sensors import ActiveSensor -import os -import cv2 -class CarlaVisionSensor(ActiveSensor): - def __init__(self, offset=(0, 0, 0), rotation=(0, 0, 0), attributes=None, - blueprint='sensor.camera.rgb', convert=None, record_npy=False): +class CarlaVisionSensor(ActiveSensor): + def __init__( + self, + offset=(0, 0, 0), + rotation=(0, 0, 0), + attributes=None, + blueprint="sensor.camera.rgb", + convert=None, + record_npy=False, + ): super().__init__() - self.transform = carla.Transform(carla.Location(x=offset[0], y=offset[1], z=offset[2]), - carla.Rotation(pitch=rotation[0], yaw=rotation[1], roll=rotation[2])) + self.transform = carla.Transform( + carla.Location(x=offset[0], y=offset[1], z=offset[2]), + carla.Rotation(pitch=rotation[0], yaw=rotation[1], roll=rotation[2]), + ) self.blueprint = blueprint if isinstance(attributes, str): - raise NotImplementedError("String parsing for attributes is not yet implemented. Feel free to do so.") + raise NotImplementedError( + "String parsing for attributes is not yet implemented. Feel free to do so." + ) elif isinstance(attributes, dict): self.attributes = attributes else: @@ -50,13 +61,19 @@ def save_last_observation(self, save_path, frame_number=None): class CarlaRGBSensor(CarlaVisionSensor): - def __init__(self, offset=(0, 0, 0), rotation=(0, 0, 0), attributes=None, - record_npy=False): - super().__init__(offset=offset, rotation=rotation, attributes=attributes, - blueprint='sensor.camera.rgb', convert=None, record_npy=record_npy) + def __init__( + self, offset=(0, 0, 0), rotation=(0, 0, 0), attributes=None, record_npy=False + ): + super().__init__( + offset=offset, + rotation=rotation, + attributes=attributes, + blueprint="sensor.camera.rgb", + convert=None, + record_npy=record_npy, + ) def processing(self, data): - array = np.frombuffer(data.raw_data, dtype=np.dtype("uint8")) array = np.reshape(array, (data.height, data.width, 4)) # BGRA format array = array[:, :, :3] # Take only RGB @@ -75,11 +92,22 @@ def save_last_observation(self, save_path, frame_number=None): class CarlaSSSensor(CarlaVisionSensor): - def __init__(self, offset=(0, 0, 0), rotation=(0, 0, 0), attributes=None, convert=None, - record_npy=False): - super().__init__(offset=offset, rotation=rotation, attributes=attributes, - blueprint='sensor.camera.semantic_segmentation', convert=convert, - record_npy=record_npy) + def __init__( + self, + offset=(0, 0, 0), + rotation=(0, 0, 0), + attributes=None, + convert=None, + record_npy=False, + ): + super().__init__( + offset=offset, + rotation=rotation, + attributes=attributes, + blueprint="sensor.camera.semantic_segmentation", + convert=convert, + record_npy=record_npy, + ) def processing(self, data): if self.convert is not None: diff --git a/src/scenic/simulators/carla/simulator.py b/src/scenic/simulators/carla/simulator.py index d37f91c5b..7e710ab0f 100644 --- a/src/scenic/simulators/carla/simulator.py +++ b/src/scenic/simulators/carla/simulator.py @@ -29,16 +29,16 @@ class CarlaSimulator(DrivingSimulator): """Implementation of `Simulator` for CARLA.""" def __init__( - self, - carla_map, - map_path, - address="127.0.0.1", - port=2000, - timeout=10, - render=True, - record="", - timestep=0.1, - traffic_manager_port=None, + self, + carla_map, + map_path, + address="127.0.0.1", + port=2000, + timeout=10, + render=True, + record="", + timestep=0.1, + traffic_manager_port=None, ): super().__init__() verbosePrint(f"Connecting to CARLA on port {port}") @@ -254,7 +254,9 @@ def createObjectInSimulator(self, obj): if sensor_bp.has_attribute(key): sensor_bp.set_attribute(key, str(val)) - carla_sensor = self.world.spawn_actor(sensor_bp, sensor.transform, attach_to=obj.carlaActor) + carla_sensor = self.world.spawn_actor( + sensor_bp, sensor.transform, attach_to=obj.carlaActor + ) carla_sensor.listen(sensor.on_data) sensor.carla_sensor = carla_sensor obj.observations = {} From e8b8e0499f0daa80eef7dee6b88b4ecf9b306696 Mon Sep 17 00:00:00 2001 From: Daniel Fremont Date: Thu, 1 May 2025 10:56:31 -0700 Subject: [PATCH 4/6] [WIP] revise sensors implementation; extend record statement --- .../data_generation/data_generation.scenic | 5 +- src/scenic/__main__.py | 7 +- src/scenic/core/dynamics/scenarios.py | 45 ++- src/scenic/core/object_types.py | 12 +- src/scenic/core/requirements.py | 5 +- src/scenic/core/sensors.py | 299 ++++++++++++++++-- src/scenic/core/simulators.py | 24 +- src/scenic/domains/driving/model.scenic | 13 - src/scenic/simulators/carla/sensors.py | 71 +---- src/scenic/simulators/carla/simulator.py | 3 +- src/scenic/syntax/ast.py | 18 +- src/scenic/syntax/compiler.py | 49 ++- src/scenic/syntax/scenic.gram | 8 +- src/scenic/syntax/veneer.py | 54 +++- tests/syntax/test_compiler.py | 36 +++ tests/syntax/test_parser.py | 57 ++++ 16 files changed, 554 insertions(+), 152 deletions(-) diff --git a/examples/sensors/carla/data_generation/data_generation.scenic b/examples/sensors/carla/data_generation/data_generation.scenic index 7a3d5740b..69d31cf70 100644 --- a/examples/sensors/carla/data_generation/data_generation.scenic +++ b/examples/sensors/carla/data_generation/data_generation.scenic @@ -27,7 +27,4 @@ other = new Car offset by 0 @ Range(10, 30), with behavior AutopilotBehavior() record ego.observations["front_rgb"] as "front_rgb" -#record ego.observations["front_rgb"] as "front_rgb" after 5s every 1s # TODO Implement -#record ego.observations["front_rgb"] after 5s every 1s to localPath("data/generation") # TODO Implement - -require monitor RecordingMonitor(ego, path=localPath("data"), recording_start=5, subsample=5) +record ego.observations["front_rgb"] every 1 seconds after 5 seconds to "data/front{time}.jpg" diff --git a/src/scenic/__main__.py b/src/scenic/__main__.py index dd72bf8b4..05f527fba 100644 --- a/src/scenic/__main__.py +++ b/src/scenic/__main__.py @@ -223,7 +223,7 @@ def generateScene(maxIterations=2000): return scene, iterations -def runSimulation(scene): +def runSimulation(scene, sceneCount): startTime = time.time() if args.verbosity >= 1: print(f" Beginning simulation of {scene.dynamicScenario}...") @@ -232,6 +232,7 @@ def runSimulation(scene): lambda: simulator.simulate( scene, maxSteps=args.time, + name=str(sceneCount), verbosity=args.verbosity, maxIterations=args.max_sims_per_scene, ) @@ -267,11 +268,13 @@ def runSimulation(scene): "(try installing python3-tk)" ) + sceneCount = 0 successCount = 0 while True: scene, _ = generateScene() + sceneCount += 1 if args.simulate: - success = runSimulation(scene) + success = runSimulation(scene, sceneCount) if success: successCount += 1 else: diff --git a/src/scenic/core/dynamics/scenarios.py b/src/scenic/core/dynamics/scenarios.py index 61a55e48c..e5f1266f4 100644 --- a/src/scenic/core/dynamics/scenarios.py +++ b/src/scenic/core/dynamics/scenarios.py @@ -5,6 +5,7 @@ import dataclasses import functools import inspect +import types import weakref import rv_ltl @@ -183,8 +184,9 @@ def _start(self): # Compute time limit now that we know the simulation timestep self._elapsedTime = 0 self._timeLimitInSteps = self._timeLimit + timestep = veneer.currentSimulation.timestep if self._timeLimitIsInSeconds: - self._timeLimitInSteps /= veneer.currentSimulation.timestep + self._timeLimitInSteps /= timestep # create monitors for each requirement used for this simulation self._requirementMonitors = [r.toMonitor() for r in self._temporalRequirements] @@ -210,6 +212,14 @@ def _start(self): for monitor in self._monitors: monitor._start() + # Prepare recorders + simName = veneer.currentSimulation.name + globalParams = types.MappingProxyType(veneer._globalParameters) + for req in self._recordedExprs: + if recConfig := req.recConfig: + recorder = recConfig.recorder + recorder.beginRecording(recConfig, simName, timestep, globalParams) + def _step(self): """Execute the (already-started) scenario for one time step. @@ -301,13 +311,25 @@ def _stop(self, reason, quiet=False): veneer.endScenario(self, reason, quiet=quiet) super()._stop(reason) - # Reject if a temporal requirement was not satisfied. + # Check if a temporal requirement was not satisfied. + rejection = None if not quiet: for req in self._requirementMonitors: if req.lastValue.is_falsy: - raise RejectSimulationException(str(req)) + rejection = str(req) + break self._requirementMonitors = None + # Stop recorders. + cancelRecordings = quiet or rejection is not None + for req in self._recordedExprs: + if recConfig := req.recConfig: + recConfig.recorder.endRecording(canceled=cancelRecordings) + + # If a temporal requirement was violated, reject (now that we're cleaned up). + if rejection is not None: + raise RejectSimulationException(rejection) + return reason def _invokeInner(self, agent, subs): @@ -333,7 +355,7 @@ def _invokeInner(self, agent, subs): # Check if any sub-scenarios stopped during action execution self._subScenarios = [sub for sub in self._subScenarios if sub._isRunning] - def _evaluateRecordedExprs(self, ty): + def _evaluateRecordedExprs(self, ty, step): if ty is RequirementType.record: place = "_recordedExprs" elif ty is RequirementType.recordInitial: @@ -342,14 +364,17 @@ def _evaluateRecordedExprs(self, ty): place = "_recordedFinalExprs" else: assert False, "invalid record type requested" - return self._evaluateRecordedExprsAt(place) + return self._evaluateRecordedExprsAt(place, step) - def _evaluateRecordedExprsAt(self, place): + def _evaluateRecordedExprsAt(self, place, step): values = {} for rec in getattr(self, place): - values[rec.name] = rec.evaluate() + value = rec.evaluate() + values[rec.name] = value + if recorder := rec.recConfig.recorder: + recorder._record(value, step) for sub in self._subScenarios: - subvals = sub._evaluateRecordedExprsAt(place) + subvals = sub._evaluateRecordedExprsAt(place, step) values.update(subvals) return values @@ -407,9 +432,9 @@ def _registerObject(self, obj): obj._parentScenario = weakref.ref(self) - def _addRequirement(self, ty, reqID, req, line, name, prob): + def _addRequirement(self, ty, reqID, req, line, name, prob, recConfig=None): """Save a requirement defined at compile-time for later processing.""" - preq = PendingRequirement(ty, req, line, prob, name, self._ego) + preq = PendingRequirement(ty, req, line, prob, name, self._ego, recConfig) self._pendingRequirements.append((reqID, preq)) def _addDynamicRequirement(self, ty, req, line, name): diff --git a/src/scenic/core/object_types.py b/src/scenic/core/object_types.py index f5c4a4a5f..543ffff33 100644 --- a/src/scenic/core/object_types.py +++ b/src/scenic/core/object_types.py @@ -1084,8 +1084,8 @@ class Object(OrientedPoint): # weakref to scenario which created this object, for internal use "_parentScenario": None, # Sensor properties - "sensors": {}, - "observations": {}, + "sensors": PropertyDefault((), {}, lambda self: {}), + "observations": PropertyDefault((), {"final"}, lambda self: {}), } def __new__(cls, *args, **kwargs): @@ -1723,14 +1723,6 @@ def _boundingPolygon(self): return self.occupiedSpace._boundingPolygon - def save_observations(self, save_path, frame_number): - import os - - for key, sensor in self.sensors.items(): - sensor_path = os.path.join(save_path, key) - os.makedirs(sensor_path, exist_ok=True) - sensor.save_last_observation(save_path=sensor_path, frame_number=frame_number) - _unitBox = shapely.geometry.Polygon(((0.5, 0.5), (-0.5, 0.5), (-0.5, -0.5), (0.5, -0.5))) diff --git a/src/scenic/core/requirements.py b/src/scenic/core/requirements.py index 287ec98c2..53633057e 100644 --- a/src/scenic/core/requirements.py +++ b/src/scenic/core/requirements.py @@ -39,12 +39,13 @@ def constrainsSampling(self): class PendingRequirement: - def __init__(self, ty, condition, line, prob, name, ego): + def __init__(self, ty, condition, line, prob, name, ego, recConfig): self.ty = ty self.condition = condition self.line = line self.prob = prob self.name = name + self.recConfig = recConfig # the translator wrapped the requirement in a lambda to prevent evaluation, # so we need to save the current values of all referenced names; we save @@ -187,6 +188,7 @@ def __init__(self, compiledReq, sample, proposition): self.closure = compiledReq.closure self.line = compiledReq.line self.name = compiledReq.name + self.recConfig = compiledReq.recConfig self.sample = sample self.compiledReq = compiledReq self.proposition = proposition @@ -440,6 +442,7 @@ def __init__(self, pendingReq, closure, dependencies, proposition): self.line = pendingReq.line self.name = pendingReq.name self.prob = pendingReq.prob + self.recConfig = pendingReq.recConfig self.dependencies = dependencies self.proposition = proposition diff --git a/src/scenic/core/sensors.py b/src/scenic/core/sensors.py index f6514a9fe..1a5db511b 100644 --- a/src/scenic/core/sensors.py +++ b/src/scenic/core/sensors.py @@ -1,29 +1,290 @@ -import copy +"""Sensors which can gather and save data from simulations.""" +import abc +from dataclasses import dataclass +import math +import os.path +import pickle +from typing import Literal, Tuple -class Sensor: - def get_observation(self): - raise NotImplementedError("Not implemented for passive sensors") +import PIL.Image +import cv2 +import numpy as np - def save_last_observation(self, save_path, frame_number=None): - raise NotImplementedError("Implement in subclass") +class Sensor(abc.ABC): + @abc.abstractmethod + def getObservation(self): + raise NotImplementedError -class ActiveSensor(Sensor): - def __init__(self): - self.observation = None - self.frame = 0 - def get_observation(self): +NO_OBSERVATION = object() + + +class CallbackSensor(Sensor): + def __init__(self, defaultValue=NO_OBSERVATION): + self._lastObservation = defaultValue + + def getObservation(self): + if self._lastObservation is NO_OBSERVATION: + raise RuntimeError( + f"{type(self).__name__} callback not called before first observation" + ) + return self.observation - def on_data(self, data): - print("On data", self.__class__) - print("Frame", data.frame) - self.observation, self.frame = self.processing(data) + def onData(self, data): + self._lastObservation = self.process(data) + + @abc.abstractmethod + def process(self, data): + raise NotImplementedError + + +class GroundTruthSensor(Sensor): + def __init__(self, value): + self._value = value + + def getObservation(self): + return self._value() + + +# Recorders + + +@dataclass +class RecordingConfiguration: + name: str + period: Tuple[float, Literal["seconds", "steps"]] + delay: Tuple[float, Literal["seconds", "steps"]] + + recorder: "Optional[Recorder]" = None + + +class Recorder: + def __init__(self): + self._recording = False + + def beginRecording(self, config, simulationName, timestep, globalParams): + assert not self._recording + self._recording = True + self.simulationName = simulationName + self.recordName = config.name + self.timestep = timestep + self.globalParams = globalParams + + val, unit = config.period + if unit == "steps": + assert isinstance(val, int) and val >= 1, val + self._period = val + else: # unit == "seconds" + assert val > 0, val + self._period = max(1, math.floor(val / timestep)) + + val, unit = config.delay + assert val >= 0, val + if unit == "steps": + assert isinstance(val, int), val + self._delay = val + else: # unit == "seconds" + self._delay = max(0, math.floor(val / timestep)) + + def recordValue(self, value, step): + raise NotImplementedError + + def endRecording(self, canceled): + assert self._recording + self._recording = False + + def _record(self, value, step): + if step >= self._delay and step % self._period == 0: + self.recordValue(np.asarray(value), step) + + @staticmethod + def _forPattern(pattern): + p0 = Recorder._formatPattern(pattern, "s", 0, 0) + p1 = Recorder._formatPattern(pattern, "s", 1, 1) + if p0 != p1: # pattern depends on step or time + return Files(pattern) + else: + return File(pattern) + + @staticmethod + def _formatPattern(pattern, simName, step, time): + return pattern.format( + simName, step, time, simulation=simName, step=step, time=time + ) + + def makePath(self, step, time): + path = self.pattern + if recordFolder := self.globalParams.get("recordFolder"): + path = os.path.join(recordFolder, path) + path = self._formatPattern(path, self.simulationName, step, time) + return path + + +class AccumulatingRecorder(Recorder): + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self._series = [] + + def recordTimeSeries(self, series): + raise NotImplementedError + + def recordValue(self, value, step): + self._series.append((step, value)) + + def endRecording(self, canceled): + if not canceled: + self.recordTimeSeries(self._series) + self._series.clear() + super().endRecording(canceled) + + +valueExtHandlers = {} #: Handlers for saving individual values to files, by extension +seriesExtHandlers = {} #: Handlers for saving time series to files, by extension + + +def prepareImageData(data, intOnly=False): + shape = data.shape + noColor = False + if len(shape) == 2: + noColor = True + elif len(shape) != 3 or shape[2] != 3: + raise TypeError(f"image data must be a 2D array of intensities or RGB triples") + dtype = data.dtype + kind = dtype.kind + if kind == "u" or kind == "i": + if np.min(data) < 0 or np.max(data) > 255: + raise ValueError(f"integer pixel data must be in the range 0-255") + return data.astype("uint8", copy=False) + elif kind == "f": + if np.min(data) < 0 or np.max(data) > 1: + raise ValueError(f"floating-point pixel data must be in the range 0-1") + if noColor and not intOnly: + return data.astype("float32", copy=False) + else: + # PIL doesn't support RGB floating-point, so round to integers + return np.round(data * 255).astype("uint8") + elif kind == "b": + if intOnly: + return (data * 255).astype("uint8") + else: + return data + else: + raise TypeError(f"cannot create image from data with dtype {dtype.name}") + + +def pilHandler(path, value, options): + image = PIL.Image.fromarray(prepareImageData(value)) + image.save(path, **options) + + +for ext, name in PIL.Image.registered_extensions().items(): + if name in PIL.Image.SAVE: + valueExtHandlers[ext[1:]] = pilHandler + + +def npyHandler(path, value, options): + np.save(path, value, **options) + + +valueExtHandlers["npy"] = npyHandler + + +def fileHandler(*exts): + def decorator(handler): + for ext in exts: + assert ext and not ext[0] == ".", ext + seriesExtHandlers[ext] = handler + return handler + + return decorator + + +@fileHandler("mkv", "mov", "mp4") +def videoHandler(path, values, timestep, options): + codec = options.get("codec") + if codec is None: + # Pragmatic choice for wider playability; may switch to always using AV1 later + codec = "avc1" if path.endswith((".mp4", ".mov")) else "AV01" + elif len(codec) != 4: + raise ValueError("video codec must be a 4-character string (FourCC)") + fourcc = cv2.VideoWriter_fourcc(*codec) + frameSize = values[0][1].shape[:2] + writer = cv2.VideoWriter(path, fourcc, 1.0 / timestep, frameSize) + try: + for step, value in values: + frame = cv2.cvtColor(prepareImageData(value, intOnly=True), cv2.COLOR_RGB2BGR) + writer.write(frame) + finally: + writer.release() + + +@fileHandler("npz") +def npzHandler(path, values, timestep, options): + timesteps, values = zip(*values) + np.savez_compressed(path, timesteps=timesteps, values=values) + + +@fileHandler("pickle") +def pickleHandler(path, values, timestep, options): + with open(path, "wb") as outFile: + pickle.dump(values, outFile, **options) + + +class File(AccumulatingRecorder): + def __init__(self, pattern, /, **options): + super().__init__() + self.pattern = pattern + _, ext = os.path.splitext(pattern) + handler = seriesExtHandlers.get(ext[1:]) + if handler is None: + if ext: + raise ValueError( + f'unknown file extension "{ext}" for recording a time series' + ) + handler = npzHandler + self.handler = handler + self.options = options + + def recordTimeSeries(self, series): + path = self.makePath("series", "series") + if dirname := os.path.dirname(path): + os.makedirs(dirname, exist_ok=True) + self.handler(path, series, self.timestep, self.options) + + +class Files(Recorder): + def __init__(self, pattern, /, **options): + super().__init__() + self.pattern = pattern + _, ext = os.path.splitext(pattern) + if not ext: + ext = ".npy" + self.pattern += ".npy" + self.handler = valueExtHandlers.get(ext[1:]) + if self.handler is None: + raise ValueError( + f'unknown file extension "{ext}" for recording a single value' + ) + self.options = options + self._paths = [] - def processing(self, data): - raise NotImplementedError("Implement in subclass") + def recordValue(self, value, step): + time = step * self.timestep + path = self.makePath(step, time) + self._paths.append(path) + if dirname := os.path.dirname(path): + os.makedirs(dirname, exist_ok=True) + self.handler(path, value, self.options) - def save_last_observation(self, save_path, frame_number=None): - raise NotImplementedError("Implement in subclass") + def endRecording(self, canceled): + if canceled: + for path in self._paths: + try: + os.remove(path) + except FileNotFoundError: + pass + self._paths.clear() + super().endRecording(canceled) diff --git a/src/scenic/core/simulators.py b/src/scenic/core/simulators.py index 29d2aab0b..3e9c0308f 100644 --- a/src/scenic/core/simulators.py +++ b/src/scenic/core/simulators.py @@ -80,6 +80,7 @@ def simulate( maxIterations=1, *, timestep=None, + name=None, verbosity=None, raiseGuardViolations=False, replay=None, @@ -103,6 +104,8 @@ def simulate( default provided by the simulator interface. Some interfaces may not allow arbitrary time step lengths or may require the timestep to be set when creating the `Simulator` and not customized per-simulation. + name (str): Name of the simulation, if any. Used to identify the simulation + when saving records to files and in debugging messages. verbosity (int): If not `None`, override Scenic's global verbosity level (from the :option:`--verbosity` option or `scenic.setDebuggingOptions`). raiseGuardViolations (bool): Whether violations of preconditions/invariants @@ -175,10 +178,14 @@ def simulate( simulation = None while not simulation and (maxIterations is None or iterations < maxIterations): iterations += 1 + if maxIterations == 1: + simName = str(name) if name else "1" + else: + simName = f"{name}.{iterations}" if name else str(iterations) simulation = self._runSingleSimulation( scene, maxSteps, - name=iterations, + name=simName, verbosity=verbosity, timestep=timestep, raiseGuardViolations=raiseGuardViolations, @@ -378,7 +385,9 @@ def __init__( scenario._stop("simulation terminated") # Record finally-recorded values. - values = dynamicScenario._evaluateRecordedExprs(RequirementType.recordFinal) + values = dynamicScenario._evaluateRecordedExprs( + RequirementType.recordFinal, self.currentTime + ) for name, val in values.items(): self.records[name] = val @@ -429,7 +438,7 @@ def _run(self, dynamicScenario, maxSteps): if not obj.sensors: continue obj.observations.update( - {key: sensor.get_observation() for key, sensor in obj.sensors.items()} + {key: sensor.getObservation() for key, sensor in obj.sensors.items()} ) # Record current state of the simulation @@ -592,13 +601,16 @@ def recordCurrentState(self): records = self.records # Record initially-recorded values - if self.currentTime == 0: - values = dynamicScenario._evaluateRecordedExprs(RequirementType.recordInitial) + step = self.currentTime + if step == 0: + values = dynamicScenario._evaluateRecordedExprs( + RequirementType.recordInitial, step + ) for name, val in values.items(): records[name] = val # Record time-series values - values = dynamicScenario._evaluateRecordedExprs(RequirementType.record) + values = dynamicScenario._evaluateRecordedExprs(RequirementType.record, step) for name, val in values.items(): records[name].append((self.currentTime, val)) diff --git a/src/scenic/domains/driving/model.scenic b/src/scenic/domains/driving/model.scenic index 2ac55b489..988d03072 100644 --- a/src/scenic/domains/driving/model.scenic +++ b/src/scenic/domains/driving/model.scenic @@ -369,16 +369,3 @@ def withinDistanceToObjsInLane(vehicle, thresholdDistance): if (distance from vehicle to obj) < thresholdDistance: return True return False - - -monitor RecordingMonitor(car, path, recording_start=1, subsample=1): - # Exclude the first recording_start steps, e.g., because cars are spawned in the air and need to drop down first - for _ in range(1, recording_start): - wait - - frame_number = 1 - while True: - car.save_observations(path, frame_number=frame_number) - frame_number += 1 - for _ in range(subsample): - wait diff --git a/src/scenic/simulators/carla/sensors.py b/src/scenic/simulators/carla/sensors.py index 42af9cdb2..dc1ea2871 100644 --- a/src/scenic/simulators/carla/sensors.py +++ b/src/scenic/simulators/carla/sensors.py @@ -5,18 +5,16 @@ import cv2 import numpy as np -from scenic.core.sensors import ActiveSensor +from scenic.core.sensors import CallbackSensor -class CarlaVisionSensor(ActiveSensor): +class CarlaVisionSensor(CallbackSensor): def __init__( self, offset=(0, 0, 0), rotation=(0, 0, 0), attributes=None, - blueprint="sensor.camera.rgb", convert=None, - record_npy=False, ): super().__init__() self.transform = carla.Transform( @@ -40,64 +38,25 @@ def __init__( elif isinstance(convert, str): self.convert = carla.ColorConverter.names[convert] else: - AttributeError("'convert' has to be int or string.") + raise TypeError("'convert' has to be int or string.") - self.record_npy = record_npy - - def save_last_observation(self, save_path, frame_number=None): - raise NotImplementedError() + blueprint = "sensor.camera.rgb" class CarlaRGBSensor(CarlaVisionSensor): - def __init__( - self, offset=(0, 0, 0), rotation=(0, 0, 0), attributes=None, record_npy=False - ): - super().__init__( - offset=offset, - rotation=rotation, - attributes=attributes, - blueprint="sensor.camera.rgb", - convert=None, - record_npy=record_npy, - ) - - def processing(self, data): + def process(self, data): array = np.frombuffer(data.raw_data, dtype=np.dtype("uint8")) array = np.reshape(array, (data.height, data.width, 4)) # BGRA format array = array[:, :, :3] # Take only RGB array = array[:, :, ::-1] # Revert order - return array, data.frame - - def save_last_observation(self, save_path, frame_number=None): - if frame_number is None: - frame_number = self.frame - save_as = os.path.join(save_path, f"{frame_number}") - if self.record_npy: - np.save(save_as, self.observation) - else: - cv2.imwrite(f"{save_as}.png", self.observation[..., ::-1]) + return array.copy() class CarlaSSSensor(CarlaVisionSensor): - def __init__( - self, - offset=(0, 0, 0), - rotation=(0, 0, 0), - attributes=None, - convert=None, - record_npy=False, - ): - super().__init__( - offset=offset, - rotation=rotation, - attributes=attributes, - blueprint="sensor.camera.semantic_segmentation", - convert=convert, - record_npy=record_npy, - ) + blueprint = "sensor.camera.semantic_segmentation" - def processing(self, data): + def process(self, data): if self.convert is not None: data.convert(self.convert) @@ -110,16 +69,4 @@ def processing(self, data): else: array = array[:, :, 2] # Take only R - return array, data.frame - - def save_last_observation(self, save_path, frame_number=None): - if frame_number is None: - frame_number = self.observation.frame - save_as = os.path.join(save_path, f"{frame_number}") - if self.record_npy: - np.save(save_as, self.observation) - else: - if self.convert is not None: - cv2.imwrite(f"{save_as}.png", self.observation[..., ::-1]) - else: - cv2.imwrite(f"{save_as}.png", self.observation) + return array.copy() diff --git a/src/scenic/simulators/carla/simulator.py b/src/scenic/simulators/carla/simulator.py index d3404f4bb..ce7bf843f 100644 --- a/src/scenic/simulators/carla/simulator.py +++ b/src/scenic/simulators/carla/simulator.py @@ -261,9 +261,8 @@ def createObjectInSimulator(self, obj): carla_sensor = self.world.spawn_actor( sensor_bp, sensor.transform, attach_to=obj.carlaActor ) - carla_sensor.listen(sensor.on_data) + carla_sensor.listen(sensor.onData) sensor.carla_sensor = carla_sensor - obj.observations = {} return carlaActor diff --git a/src/scenic/syntax/ast.py b/src/scenic/syntax/ast.py index e7781e1a5..4b31bceab 100644 --- a/src/scenic/syntax/ast.py +++ b/src/scenic/syntax/ast.py @@ -1,6 +1,6 @@ import ast import typing -from typing import Optional, Union +from typing import Any, Optional, Union class AST(ast.AST): @@ -355,15 +355,25 @@ def __reduce__(self): class Record(AST): - __match_args__ = ("value", "name") + __match_args__ = ("value", "name", "recorder", "period", "delay") def __init__( - self, value: ast.AST, name: Optional[str] = None, *args: any, **kwargs: any + self, + value: ast.AST, + name: Optional[str] = None, + recorder: Any = None, + period: Optional[Union["Seconds", "Steps"]] = None, + delay: Optional[Union["Seconds", "Steps"]] = None, + *args: Any, + **kwargs: Any, ) -> None: super().__init__(*args, **kwargs) self.value = value self.name = name - self._fields = ["value", "name"] + self.recorder = recorder + self.period = period + self.delay = delay + self._fields = ["value", "name", "recorder", "period", "delay"] class RecordInitial(AST): diff --git a/src/scenic/syntax/compiler.py b/src/scenic/syntax/compiler.py index 70f1b9f24..4be8e3b27 100644 --- a/src/scenic/syntax/compiler.py +++ b/src/scenic/syntax/compiler.py @@ -2,7 +2,7 @@ from copy import copy from enum import IntFlag, auto import itertools -from typing import Any, Callable, List, Literal, Optional, Tuple, Union +from typing import Any, Callable, Dict, List, Literal, Optional, Tuple, Union from scenic.core.errors import ScenicParseError, getText import scenic.syntax.ast as s @@ -1163,10 +1163,15 @@ def visit_Param(self, node: s.Param): def visit_Require(self, node: s.Require): prob = node.prob - if prob is not None and not 0 <= prob <= 1: - raise self.makeSyntaxError("probability must be between 0 and 1", node) + if prob is not None: + if not 0 <= prob <= 1: + raise self.makeSyntaxError("probability must be between 0 and 1", node) + kwargs = {"prob": ast.Constant(prob)} + else: + kwargs = {} + return self.createRequirementLike( - "require", node.cond, node.lineno, node.name, prob + "require", node.cond, node.lineno, node.name, kwargs ) def visit_RequireMonitor(self, node: s.RequireMonitor): @@ -1322,7 +1327,23 @@ def makeDoLike( @context(Context.TOP_LEVEL) def visit_Record(self, node: s.Record): - return self.createRequirementLike("record", node.value, node.lineno, node.name) + if node.name and node.recorder: + raise self.makeSyntaxError( + 'cannot use both "as" and "to" in "record" statement', node + ) + kwargs = {} + if node.recorder: + kwargs["recorder"] = self.visit(node.recorder) + if node.period: + elts = [self.visit(node.period.value), ast.Constant(node.period.unitStr)] + kwargs["period"] = ast.Tuple(elts, loadCtx) + if node.delay: + elts = [self.visit(node.delay.value), ast.Constant(node.delay.unitStr)] + kwargs["delay"] = ast.Tuple(elts, loadCtx) + + return self.createRequirementLike( + "record", node.value, node.lineno, node.name, kwargs + ) @context(Context.TOP_LEVEL) def visit_RecordInitial(self, node: s.RecordInitial): @@ -1354,7 +1375,7 @@ def createRequirementLike( body: ast.AST, lineno: int, name: Optional[str] = None, - prob: Optional[float] = None, + kwargs: Dict[str, Union[Tuple[ast.AST, ...], ast.AST]] = {}, ): """Create a call to a function that implements requirement-like features, such as `record` and `terminate when`. @@ -1363,14 +1384,20 @@ def createRequirementLike( must be `(reqId: int, body: () -> bool, lineno: int, name: str | None)` body (ast.AST): AST node to evaluate for checking the condition lineno (int): Line number in the source code - name (Optional[str]): Optional name for requirements. Defaults to None. - prob (Optional[float]): Optional probability for requirements. Defaults to None. + name (Optional[str], optional): Optional name for requirements. Defaults to None. + kwargs: Optional keyword arguments. """ propTransformer = PropositionTransformer(self.filename) newBody, self.nextSyntaxId = propTransformer.transform(body, self.nextSyntaxId) newBody = self.visit(newBody) requirementId = self._register_requirement_syntax(body) + keywords = [] + for datum, node in kwargs.items(): + if isinstance(node, tuple): + node = ast.Tuple(*node) + keywords.append(ast.keyword(arg=datum, value=node)) + return ast.Expr( value=ast.Call( func=ast.Name(functionName, loadCtx), @@ -1380,11 +1407,7 @@ def createRequirementLike( ast.Constant(lineno), # line number ast.Constant(name), # requirement name ], - keywords=( - [ast.keyword(arg="prob", value=ast.Constant(prob))] - if prob is not None - else [] - ), + keywords=keywords, ) ) diff --git a/src/scenic/syntax/scenic.gram b/src/scenic/syntax/scenic.gram index 0033bd59e..9ac0527d2 100644 --- a/src/scenic/syntax/scenic.gram +++ b/src/scenic/syntax/scenic.gram @@ -2125,8 +2125,12 @@ scenic_require_stmt_name: | a=STRING { a.string[1:-1] } scenic_record_stmt: - | "record" e=expression n=['as' a=scenic_require_stmt_name { a }] { - s.Record(value=e, name=n, LOCATIONS) + | "record" e=expression \ + p=["every" a=scenic_dynamic_duration { a }] \ + d=["after" a=scenic_dynamic_duration { a }] \ + n=['as' a=scenic_require_stmt_name { a }] \ + r=['to' a=expression { a }] { + s.Record(value=e, name=n, recorder=r, delay=d, period=p, LOCATIONS) } scenic_record_initial_stmt: diff --git a/src/scenic/syntax/veneer.py b/src/scenic/syntax/veneer.py index 4b550fbdd..cce52162f 100644 --- a/src/scenic/syntax/veneer.py +++ b/src/scenic/syntax/veneer.py @@ -120,6 +120,8 @@ "VerifaiRange", "VerifaiDiscreteRange", "VerifaiOptions", + "File", + "Files", # Constructible types "Point", "OrientedPoint", @@ -222,6 +224,7 @@ everywhere, nowhere, ) +from scenic.core.sensors import File, Files from scenic.core.shapes import ( BoxShape, ConeShape, @@ -279,6 +282,7 @@ import scenic.core.propositions as propositions from scenic.core.regions import convertToFootprint import scenic.core.requirements as requirements +from scenic.core.sensors import Recorder, RecordingConfiguration from scenic.core.simulators import RejectSimulationException from scenic.core.specifiers import ModifyingSpecifier, Specifier from scenic.core.type_support import ( @@ -765,10 +769,52 @@ def require_monitor(reqID, value, line, name): ) -def record(reqID, value, line, name): +def record(reqID, value, line, name, recorder=None, period=None, delay=None): if not name: name = f"record{line}" - makeRequirement(requirements.RequirementType.record, reqID, value, line, name) + if recorder is not None: + if isinstance(recorder, str): + recorder = Recorder._forPattern(recorder) + if not isinstance(recorder, Recorder): + raise TypeError( + f'"record X to Y" on line {line} with Y not a str or Recorder' + ) + if period is not None: + val, unit = period + if not isinstance(val, numbers.Real): + raise TypeError( + f'period of "record" statement on line {line} must be a number' + ) + if val <= 0: + raise ValueError( + f'period of "record" statement on line {line} must be positive' + ) + if unit == "steps" and not isinstance(val, int): + raise TypeError( + f'"record every X steps" on line {line} with X not an integer' + ) + else: + period = (1, "steps") + if delay is not None: + val, unit = delay + if not isinstance(val, numbers.Real): + raise TypeError( + f'delay of "record" statement on line {line} must be a number' + ) + if val < 0: + raise ValueError( + f'delay of "record" statement on line {line} must be nonnegative' + ) + if unit == "steps" and not isinstance(val, int): + raise TypeError( + f'"record after X steps" on line {line} with X not an integer' + ) + else: + delay = (0, "steps") + config = RecordingConfiguration( + name=name, recorder=recorder, period=period, delay=delay + ) + makeRequirement(requirements.RequirementType.record, reqID, value, line, name, config) def record_initial(reqID, value, line, name): @@ -815,7 +861,7 @@ def terminate_simulation_when(reqID, req, line, name): ) -def makeRequirement(ty, reqID, req, line, name): +def makeRequirement(ty, reqID, req, line, name, recConfig=None): if evaluatingRequirement: raise InvalidScenarioError(f'tried to use "{ty.value}" inside a requirement') elif currentBehavior is not None: @@ -823,7 +869,7 @@ def makeRequirement(ty, reqID, req, line, name): elif currentSimulation is not None: currentScenario._addDynamicRequirement(ty, req, line, name) else: # requirement being defined at compile time - currentScenario._addRequirement(ty, reqID, req, line, name, 1) + currentScenario._addRequirement(ty, reqID, req, line, name, 1, recConfig) def terminate_after(timeLimit, terminator=None): diff --git a/tests/syntax/test_compiler.py b/tests/syntax/test_compiler.py index 1af4dbf9a..1264e4bd8 100644 --- a/tests/syntax/test_compiler.py +++ b/tests/syntax/test_compiler.py @@ -1461,6 +1461,42 @@ def test_record(self): case _: assert False + def test_record_clauses(self): + node, requirements = compileScenicAST( + Record( + Name("C", lineno=2), + period=Seconds(Constant(0.2)), + delay=Steps(Constant(10)), + recorder=Name("file"), + lineno=2, + ) + ) + match node: + case Expr( + Call( + Name("record"), + [ + Constant(0), # reqId + Call(Name("AtomicProposition"), [Lambda(arguments(), Name("C"))]), + Constant(2), # lineno + Constant(None), # name + ], + [ + keyword("recorder", Name("file")), + keyword("period", Tuple(Constant(0.2), Constant("seconds"))), + keyword("delay", Tuple(Constant(10), Constant("steps"))), + ], + ) + ): + assert True + case _: + assert False + match requirements: + case [Name("C")]: + assert True + case _: + assert False + def test_record_initial(self): node, requirements = compileScenicAST( RecordInitial(Name("C", lineno=2), lineno=2) diff --git a/tests/syntax/test_parser.py b/tests/syntax/test_parser.py index da5f80545..d43d5b4fd 100644 --- a/tests/syntax/test_parser.py +++ b/tests/syntax/test_parser.py @@ -1144,6 +1144,63 @@ def test_record_named(self): case _: assert False + def test_record_recorder(self): + mod = parse_string_helper("record x to file") + stmt = mod.body[0] + match stmt: + case Record(Name("x"), recorder=Name("file")): + assert True + case _: + assert False + + def test_record_delay(self): + mod = parse_string_helper("record x after 5 seconds") + stmt = mod.body[0] + match stmt: + case Record(Name("x"), delay=Seconds(Constant(5))): + assert True + case _: + assert False + + mod = parse_string_helper("record x after 5 steps") + stmt = mod.body[0] + match stmt: + case Record(Name("x"), delay=Steps(Constant(5))): + assert True + case _: + assert False + + def test_record_period(self): + mod = parse_string_helper("record x every 0.2 seconds") + stmt = mod.body[0] + match stmt: + case Record(Name("x"), period=Seconds(Constant(0.2))): + assert True + case _: + assert False + + mod = parse_string_helper("record x every 3 steps") + stmt = mod.body[0] + match stmt: + case Record(Name("x"), period=Steps(Constant(3))): + assert True + case _: + assert False + + def test_record_combined(self): + mod = parse_string_helper("record x every 0.2 seconds after 5 seconds to file") + stmt = mod.body[0] + match stmt: + case Record( + Name("x"), + period=Seconds(Constant(0.2)), + delay=Seconds(Constant(5)), + recorder=Name("file"), + ): + assert True + case _: + assert False + def test_record_initial(self): mod = parse_string_helper("record initial x") stmt = mod.body[0] From 31e194dd04a242069fd797789a80203b7d098f52 Mon Sep 17 00:00:00 2001 From: Daniel Fremont Date: Thu, 1 May 2025 14:11:39 -0700 Subject: [PATCH 5/6] various fixes --- .../carla/data_generation/data_generation.scenic | 7 +++++-- src/scenic/core/dynamics/scenarios.py | 7 +++---- src/scenic/core/sensors.py | 16 +++++++++++----- src/scenic/simulators/carla/sensors.py | 7 ++++++- 4 files changed, 25 insertions(+), 12 deletions(-) diff --git a/examples/sensors/carla/data_generation/data_generation.scenic b/examples/sensors/carla/data_generation/data_generation.scenic index 69d31cf70..470e46911 100644 --- a/examples/sensors/carla/data_generation/data_generation.scenic +++ b/examples/sensors/carla/data_generation/data_generation.scenic @@ -26,5 +26,8 @@ ego = new Car at spot, other = new Car offset by 0 @ Range(10, 30), with behavior AutopilotBehavior() -record ego.observations["front_rgb"] as "front_rgb" -record ego.observations["front_rgb"] every 1 seconds after 5 seconds to "data/front{time}.jpg" +param recordFolder = "out/{simulation}" +record ego.observations["front_ss"] every 0.5 seconds after 5 seconds to "frontss_{time}.jpg" +record ego.observations["front_rgb"] after 5 seconds to "frontrgb.mp4" + +terminate after 15 seconds diff --git a/src/scenic/core/dynamics/scenarios.py b/src/scenic/core/dynamics/scenarios.py index e5f1266f4..7905251e0 100644 --- a/src/scenic/core/dynamics/scenarios.py +++ b/src/scenic/core/dynamics/scenarios.py @@ -216,8 +216,7 @@ def _start(self): simName = veneer.currentSimulation.name globalParams = types.MappingProxyType(veneer._globalParameters) for req in self._recordedExprs: - if recConfig := req.recConfig: - recorder = recConfig.recorder + if (recConfig := req.recConfig) and (recorder := recConfig.recorder): recorder.beginRecording(recConfig, simName, timestep, globalParams) def _step(self): @@ -323,8 +322,8 @@ def _stop(self, reason, quiet=False): # Stop recorders. cancelRecordings = quiet or rejection is not None for req in self._recordedExprs: - if recConfig := req.recConfig: - recConfig.recorder.endRecording(canceled=cancelRecordings) + if (recConfig := req.recConfig) and (recorder := recConfig.recorder): + recorder.endRecording(canceled=cancelRecordings) # If a temporal requirement was violated, reject (now that we're cleaned up). if rejection is not None: diff --git a/src/scenic/core/sensors.py b/src/scenic/core/sensors.py index 1a5db511b..c19b8d787 100644 --- a/src/scenic/core/sensors.py +++ b/src/scenic/core/sensors.py @@ -6,6 +6,7 @@ import os.path import pickle from typing import Literal, Tuple +import sys import PIL.Image import cv2 @@ -31,7 +32,7 @@ def getObservation(self): f"{type(self).__name__} callback not called before first observation" ) - return self.observation + return self._lastObservation def onData(self, data): self._lastObservation = self.process(data) @@ -204,15 +205,20 @@ def decorator(handler): @fileHandler("mkv", "mov", "mp4") def videoHandler(path, values, timestep, options): + if not values: + return # empty time series codec = options.get("codec") if codec is None: - # Pragmatic choice for wider playability; may switch to always using AV1 later - codec = "avc1" if path.endswith((".mp4", ".mov")) else "AV01" + if sys.platform == "darwin": + # Pragmatic choice for wider playability; may switch to always using AV1 later + codec = "avc1" if path.endswith((".mp4", ".mov")) else "AV01" + else: + codec = "mp4v" elif len(codec) != 4: raise ValueError("video codec must be a 4-character string (FourCC)") fourcc = cv2.VideoWriter_fourcc(*codec) - frameSize = values[0][1].shape[:2] - writer = cv2.VideoWriter(path, fourcc, 1.0 / timestep, frameSize) + height, width = values[0][1].shape[:2] + writer = cv2.VideoWriter(path, fourcc, 1.0 / timestep, (width, height)) try: for step, value in values: frame = cv2.cvtColor(prepareImageData(value, intOnly=True), cv2.COLOR_RGB2BGR) diff --git a/src/scenic/simulators/carla/sensors.py b/src/scenic/simulators/carla/sensors.py index dc1ea2871..8771bef2f 100644 --- a/src/scenic/simulators/carla/sensors.py +++ b/src/scenic/simulators/carla/sensors.py @@ -21,7 +21,6 @@ def __init__( carla.Location(x=offset[0], y=offset[1], z=offset[2]), carla.Rotation(pitch=rotation[0], yaw=rotation[1], roll=rotation[2]), ) - self.blueprint = blueprint if isinstance(attributes, str): raise NotImplementedError( "String parsing for attributes is not yet implemented. Feel free to do so." @@ -40,8 +39,14 @@ def __init__( else: raise TypeError("'convert' has to be int or string.") + self.frame = 0 + blueprint = "sensor.camera.rgb" + def onData(self, data): + super().onData(data) + self.frame = data.frame + class CarlaRGBSensor(CarlaVisionSensor): def process(self, data): From 71903cb973e5234cd8f5541792216584a18d7a16 Mon Sep 17 00:00:00 2001 From: Daniel Fremont Date: Tue, 6 May 2025 17:26:48 -0700 Subject: [PATCH 6/6] run isort --- src/scenic/core/sensors.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/scenic/core/sensors.py b/src/scenic/core/sensors.py index c19b8d787..1475bd583 100644 --- a/src/scenic/core/sensors.py +++ b/src/scenic/core/sensors.py @@ -5,8 +5,8 @@ import math import os.path import pickle -from typing import Literal, Tuple import sys +from typing import Literal, Tuple import PIL.Image import cv2