diff --git a/docs/api.rst b/docs/api.rst index 440f450f2..7f33c12d2 100644 --- a/docs/api.rst +++ b/docs/api.rst @@ -173,6 +173,15 @@ it to the replay, but this greatly increases the size of the encoded simulation. can return to one later for further analysis), but it is not guaranteed to be compatible across major versions of Scenic. +.. _xosc_export: + +OpenScenarioXML Export +---------------------- + +Scenic provides experimental support for exporting completed simulations via `toOpenScenario`. +This function currently only supports cars and pedestrians, and may be subject to breaking changes +in the future. + .. seealso:: If you get exceptions or unexpected behavior when using the API, Scenic provides various debugging features: see :ref:`debugging`. .. rubric:: Footnotes diff --git a/docs/simulators.rst b/docs/simulators.rst index ee5d0b0d1..5a966660b 100644 --- a/docs/simulators.rst +++ b/docs/simulators.rst @@ -14,6 +14,10 @@ See the individual entries for details on each interface's capabilities and how While Scenic aims to support multiple Python versions, some simulators may have more limited compatibility. Be sure to check the documentation of each simulator to confirm which Python versions are supported. +.. note:: + Scenic also supports outputing data in formats that may be imported into other simulators and tools (e.g. :ref:`xosc_export`). + For more details, see :ref:`serialization`. + .. contents:: List of Simulators :local: @@ -163,7 +167,6 @@ This interface is part of the VerifAI toolkit; documentation and examples can be .. _VerifAI repository: https://github.com/BerkeleyLearnVerify/VerifAI - Deprecated ========== diff --git a/pyproject.toml b/pyproject.toml index dc667ba7a..6f6cdf907 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -60,6 +60,9 @@ metadrive = [ "metadrive-simulator >= 0.4.3", "sumolib >= 1.21.0", ] +openscenario = [ + "scenariogeneration" +] test = [ # minimum dependencies for running tests (used for tox virtualenvs) "pytest >= 7.0.0", "pytest-cov >= 3.0.0", @@ -68,6 +71,7 @@ test = [ # minimum dependencies for running tests (used for tox virtualenvs) test-full = [ # like 'test' but adds dependencies for optional features "scenic[test]", # all dependencies from 'test' extra above "scenic[guideways]", # for running guideways modules + "scenic[openscenario]", "astor >= 0.8.1", 'carla >= 0.9.12; python_version <= "3.12" and (platform_system == "Linux" or platform_system == "Windows")', "dill", diff --git a/src/scenic/core/serialization.py b/src/scenic/core/serialization.py index 580ffb32b..97c083734 100644 --- a/src/scenic/core/serialization.py +++ b/src/scenic/core/serialization.py @@ -8,12 +8,15 @@ import hashlib import io import math +import os import pickle import struct import types +import warnings from scenic.core.distributions import Samplable, needsSampling from scenic.core.utils import DefaultIdentityDict +from scenic.core.vectors import Vector def deterministicHash(mapping, *, digest_size=8): @@ -392,3 +395,224 @@ def readStr(stream): Serializer.addCodec(str, writeStr, readStr) + + +def toOpenScenario( + simulationResult, + scenario, + scene, + mapPath=None, + scenarioName="ScenicScenario", +): + """Export a `SimulationResult` as a `scenariogeneration.xosc.scenario `_ object. + + Args: + simulationResult: The `SimulationResult` to be exported to XOSC + scenario: The scenario from which simulationResult was sampled. + scene: The scene from which simulationResult was sampled. + mapPath: The path to the XODR map used to run the simulation. If + one is not provided the `map` param of the scenario is used. + scenarioName: The name of the scenario in the generated XOSC file. + """ + try: + import scenariogeneration + from scenariogeneration import ScenarioGenerator, xosc + except ModuleNotFoundError as e: + raise ModuleNotFoundError( + "The `scenariogeneration` package is required to use Scenic's XOSC export functionality." + ) from e + + # Create catalog + xosc_catalog = xosc.Catalog() + + # Create parameters + xosc_paramdec = xosc.ParameterDeclarations() + + # Extract map + if mapPath is None: + if "map" not in scenario.params: + raise ValueError( + "No `mapPath` provided and scenario does not have a `map` parameter defined." + ) + mapPath = ( + mapPath if mapPath is not None else os.path.abspath(scenario.params["map"]) + ) + xosc_road = xosc.RoadNetwork(roadfile=mapPath) + + # Create entitities + entities = xosc.Entities() + xosc_objects = {} + for obj_i, obj in enumerate(scene.objects): + if hasattr(obj, "isVehicle") and obj.isVehicle: + obj_name = obj.name if hasattr(obj, "name") else f"Vehicle{obj_i}" + veh_bb = xosc.BoundingBox( + obj.width, + obj.length, + obj.height, + 0, + 0, + 0, + ) + veh_fa = xosc.Axle( + obj.maxSteeringAngle, + obj.wheelDiameter, + obj.trackWidth, + obj.wheelbase, + obj.groundClearance, + ) + veh_ra = xosc.Axle( + obj.maxSteeringAngle, + obj.wheelDiameter, + obj.trackWidth, + 0, + obj.groundClearance, + ) + xosc_obj = xosc.Vehicle( + name=obj_name, + vehicle_type=xosc.VehicleCategory.car, + boundingbox=veh_bb, + frontaxle=veh_fa, + rearaxle=veh_ra, + max_speed=obj.maxSpeed, + max_acceleration=obj.maxAcceleration, + max_deceleration=obj.maxDeceleration, + mass=None, + model3d=None, + max_acceleration_rate=None, + max_deceleration_rate=None, + role=None, + ) + elif hasattr(obj, "isPedestrian") and obj.isPedestrian: + obj_name = obj.name if hasattr(obj, "name") else f"Pedestrian{obj_i}" + ped_bb = xosc.BoundingBox( + obj.width, + obj.length, + obj.height, + 0, + 0, + 0, + ) + xosc_obj = xosc.Pedestrian( + name=obj_name, + mass=obj.mass, + boundingbox=ped_bb, + category=xosc.PedestrianCategory.pedestrian, + model=None, + role=None, + ) + else: + warnings.warn( + f"Object {obj} of unsupported type is being ignored during XOSC export." + ) + continue + + xosc_objects[obj] = xosc_obj + entities.add_scenario_object(obj_name, xosc_obj) + + # Helper function + def pos_to_WorldPosition(obj, pos, yaw): + # XOSC Reference point is back axle, so we must translate Scenic's + # convention to this. + state_position = ( + pos.offsetRotated(yaw, Vector(0, -0.5 * obj.wheelbase, 0)) + if obj.isVehicle + else pos + ) + state_orientation = yaw + math.radians(90) + return xosc.WorldPosition( + x=state_position.x, + y=state_position.y, + z=state_position.z, + h=state_orientation, + ) + + # Initial states + init = xosc.Init() + + for obj, xosc_obj in xosc_objects.items(): + obj_init_action = xosc.TeleportAction( + pos_to_WorldPosition(obj, obj.position, obj.yaw) + ) + init.add_init_action(xosc_obj.name, obj_init_action) + + # Dynamics + xosc_act = xosc.Act( + "MainAct", + xosc.ValueTrigger( + "StartSimulation", + 0, + xosc.ConditionEdge.none, + xosc.SimulationTimeCondition(0, xosc.Rule.greaterThan), + ), + ) + + for obj_i, (obj, xosc_obj) in enumerate(xosc_objects.items()): + action_times = [] + action_positions = [] + for t, states in enumerate(simulationResult.trajectory): + action_positions.append( + pos_to_WorldPosition( + obj, states.positions[obj_i], states.orientations[obj_i].yaw + ) + ) + action_times.append(simulationResult.timestep * t) + + polyline = xosc.Polyline(time=action_times, positions=action_positions) + trajectory = xosc.Trajectory(name=f"Trajectory_{xosc_obj.name}", closed=False) + trajectory.add_shape(polyline) + + traj_action = xosc.FollowTrajectoryAction( + trajectory=trajectory, + following_mode=xosc.FollowingMode.position, + reference_domain=xosc.ReferenceContext.absolute, + scale=1, + offset=0, + ) + + event = xosc.Event(f"Event_{xosc_obj.name}", xosc.Priority.override) + event.add_trigger( + xosc.ValueTrigger( + f"TimeTrigger_{xosc_obj.name}", + 0, + xosc.ConditionEdge.none, + xosc.SimulationTimeCondition(0, xosc.Rule.greaterThan), + ) + ) + event.add_action(f"Action_{xosc_obj.name}", action=traj_action) + + maneuver = xosc.Maneuver("Maneuver_{xosc_obj.name}") + maneuver.add_event(event) + + manuever_group = xosc.ManeuverGroup(f"ManeuverGroup_{xosc_obj.name}") + manuever_group.add_maneuver(maneuver) + manuever_group.add_actor(xosc_obj.name) + + xosc_act.add_maneuver_group(manuever_group) + + # Create storyboard + xosc_sb = xosc.StoryBoard( + init, + xosc.ValueTrigger( + "StopSimulation", + 0, + xosc.ConditionEdge.rising, + xosc.SimulationTimeCondition( + simulationResult.currentRealTime, xosc.Rule.greaterThan + ), + "stop", + ), + ) + xosc_sb.add_act(xosc_act) + + # Create scenario + xosc_scenario = xosc.Scenario( + scenarioName, + "Scenic", + xosc_paramdec, + entities=entities, + storyboard=xosc_sb, + roadnetwork=xosc_road, + catalog=xosc_catalog, + ) + + return xosc_scenario diff --git a/src/scenic/core/simulators.py b/src/scenic/core/simulators.py index 3e9c0308f..f662c451d 100644 --- a/src/scenic/core/simulators.py +++ b/src/scenic/core/simulators.py @@ -811,7 +811,19 @@ def currentState(self): The default implementation returns a tuple of the positions of all objects. """ - return tuple(obj.position for obj in self.objects) + + class SimulationState(tuple): + def __new__(cls, positions, orientations): + return super().__new__(cls, positions) + + def __init__(self, positions, orientations): + self.positions = positions + self.orientations = orientations + + positions = tuple(obj.position for obj in self.objects) + orientation = tuple(obj.orientation for obj in self.objects) + + return SimulationState(positions, orientation) @property def currentRealTime(self): diff --git a/src/scenic/domains/driving/model.scenic b/src/scenic/domains/driving/model.scenic index 40191b14b..8db0c2805 100644 --- a/src/scenic/domains/driving/model.scenic +++ b/src/scenic/domains/driving/model.scenic @@ -134,6 +134,10 @@ class DrivingObject: def isVehicle(self): return False + @property + def isPedestrian(self): + return False + @property def isCar(self): return False @@ -282,6 +286,24 @@ class Vehicle(DrivingObject): color (:obj:`Color` or RGB tuple): Color of the vehicle. The default value is a distribution derived from car color popularity statistics; see :obj:`Color.defaultCarColor`. + wheelbase: The distance between the front and rear axles of the vehicle. Default value is 0.6 + times the length of the vehicle. + maxSteeringAngle: The maximum steering angle of the vehicle. The full steering range would be + two times this value, going from (-maxSteeringAngle, maxSteeringAngle). Default value + 30 degrees. + wheelDiameter: The diameter of the *entire* wheel (including the tire). Default value is 0.7 meters. + trackWidth: Distance between the vehicle's wheels when pointed straight ahead. Default value + is 0.85 times the width of the vehicle. + groundClearance: Default value is half the wheel diameter. + maxSpeed: The maximum rated speed of the vehicle. Default value is 45 meters per second (~100 mph). + This value is not enforced by Scenic and is provided simply for other tools to reference (e.g. + exporting to OpenScenarioXML). + maxAcceleration: The maximum rated acceleration of the vehicle. Default value is 5 meters per second^2. + This value is not enforced by Scenic and is provided simply for other tools to reference (e.g. + exporting to OpenScenarioXML). + maxDeceleration: The maximum rated deceleration of the vehicle. Default value is 10 meters per second^2. + This value is not enforced by Scenic and is provided simply for other tools to reference (e.g. + exporting to OpenScenarioXML). """ regionContainedIn: roadOrShoulder position: new Point on road @@ -291,6 +313,14 @@ class Vehicle(DrivingObject): width: 2 length: 4.5 color: Color.defaultCarColor() + wheelbase: 0.6*self.length + maxSteeringAngle: 35 deg + wheelDiameter: 0.7 + trackWidth: 0.85*self.width + groundClearance: 0.5*self.wheelDiameter + maxSpeed: 45 + maxAcceleration: 5 + maxDeceleration: 10 @property def isVehicle(self): @@ -317,6 +347,7 @@ class Pedestrian(DrivingObject): length: The default length is 0.75 m. color: The default color is turquoise. Pedestrian colors are not necessarily used by simulators, but do appear in the debugging diagram. + mass: Default value is 65 kg. """ regionContainedIn: network.walkableRegion position: new Point on network.walkableRegion @@ -325,6 +356,11 @@ class Pedestrian(DrivingObject): width: 0.75 length: 0.75 color: [0, 0.5, 1] + mass: 65 + + @property + def isPedestrian(self): + return True ## Stub sensor implementations diff --git a/src/scenic/simulators/metadrive/simulator.py b/src/scenic/simulators/metadrive/simulator.py index 76607d5bc..075134f55 100644 --- a/src/scenic/simulators/metadrive/simulator.py +++ b/src/scenic/simulators/metadrive/simulator.py @@ -39,7 +39,7 @@ def __init__( timestep=0.1, render=True, render3D=False, - real_time=True, + real_time=None, screen_record=False, screen_record_filename=None, screen_record_path="metadrive_gifs", @@ -51,7 +51,10 @@ def __init__( self.timestep = timestep self.sumo_map = sumo_map self.xodr_map = xodr_map - self.real_time = real_time + if real_time is None: + self.real_time = self.render or self.render3D + else: + self.real_time = real_time self.screen_record = screen_record self.screen_record_filename = screen_record_filename self.screen_record_path = screen_record_path diff --git a/tests/core/test_serialization.py b/tests/core/test_serialization.py index 6ae1d1c95..c4ca8ca0d 100644 --- a/tests/core/test_serialization.py +++ b/tests/core/test_serialization.py @@ -13,8 +13,14 @@ import numpy import pytest -from scenic.core.serialization import SerializationError, Serializer, deterministicHash +from scenic.core.serialization import ( + SerializationError, + Serializer, + deterministicHash, + toOpenScenario, +) from scenic.core.simulators import DivergenceError, DummySimulator +from tests.simulators.metadrive.test_metadrive import getMetadriveSimulator from tests.utils import ( areEquivalent, compileScenic, @@ -507,3 +513,45 @@ class Foo: digest2 = deterministicHash(mapping2) # Non-scalar values should hash in a stable way, independent of identity. assert digest1 == digest2 + + +def test_xosc_export(getMetadriveSimulator): + simulator, openDrivePath, sumoPath = getMetadriveSimulator("Town01") + code = f""" + param map = r'{openDrivePath}' + param sumo_map = r'{sumoPath}' + + model scenic.simulators.metadrive.model + + behavior DriveAndBrakeForPedestrians(): + try: + do FollowLaneBehavior() + interrupt when withinDistanceToAnyPedestrians(self, 10): + take SetThrottleAction(0), SetBrakeAction(1) + + behavior CrossRoad(): + while distance from self to ego > 15: + wait + take SetWalkingDirectionAction(self.heading), SetWalkingSpeedAction(1) + + ego = new Car with behavior DriveAndBrakeForPedestrians() + + rightCurb = ego.laneGroup.curb + spot = new OrientedPoint on visible rightCurb + + parkedCar = new Car right of spot by 1, with regionContainedIn None + + require distance from ego to parkedCar > 30 + + new Pedestrian ahead of parkedCar by 3, + facing 90 deg relative to parkedCar, + with behavior CrossRoad() + + terminate after 30 seconds + """ + + scenario = compileScenic(code, mode2D=True, params={"map": openDrivePath}) + scene, _ = scenario.generate() + simulationResult = simulator.simulate(scene) + assert simulationResult is not None + xosc_scenario = toOpenScenario(simulationResult, scenario, scene) diff --git a/tests/simulators/metadrive/test_metadrive.py b/tests/simulators/metadrive/test_metadrive.py index 1c1ea9ad1..4fe573d2e 100644 --- a/tests/simulators/metadrive/test_metadrive.py +++ b/tests/simulators/metadrive/test_metadrive.py @@ -84,7 +84,9 @@ def test_pickle(loadLocalScenario): def getMetadriveSimulator(getAssetPath): base = getAssetPath("maps/CARLA") - def _getMetadriveSimulator(town, *, render=False, render3D=False, **kwargs): + def _getMetadriveSimulator( + town, *, render=False, render3D=False, real_time=False, **kwargs + ): openDrivePath = os.path.join(base, f"{town}.xodr") sumoPath = os.path.join(base, f"{town}.net.xml") simulator = MetaDriveSimulator( @@ -92,6 +94,7 @@ def _getMetadriveSimulator(town, *, render=False, render3D=False, **kwargs): xodr_map=openDrivePath, render=render, render3D=render3D, + real_time=real_time, **kwargs, ) return simulator, openDrivePath, sumoPath