[WIP] Parallel Behavior Support

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

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
 ==========
 

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",

src/scenic/core/dynamics/behaviors.py

@@ -109,7 +109,7 @@ def alarmHandler(signum, frame):
             try:
                 actions = self._runningIterator.send(None)
             except StopIteration:
-                actions = ()  # behavior ended early
+                return None
         return actions
 
     def _stop(self, reason=None):
@@ -124,18 +124,39 @@ def _isFinished(self):
     def _invokeInner(self, agent, subs):
         import scenic.syntax.veneer as veneer
 
-        assert len(subs) == 1
-        sub = subs[0]
-        if not isinstance(sub, Behavior):
-            raise TypeError(f"expected a behavior, got {sub}")
-        sub._start(agent)
-        with veneer.executeInBehavior(sub):
+        # Validate all inner behaviors
+        for sub in subs:
+            if not isinstance(sub, Behavior):
+                raise TypeError(f"expected a behavior, got {sub}")
+            sub._start(agent)
+
+        # Create a generator for each inner behavior that yields the appropriate actions, cleaning up when done.
+        def make_inner_generator(sub):
             try:
-                yield from sub._runningIterator
+                while sub._isRunning:
+                    actions = sub._step()
+                    if actions is None:
+                        return
+                    yield actions
             finally:
                 if sub._isRunning:
                     sub._stop()
 
+        inner_generators = [make_inner_generator(sub) for sub in subs]
+
+        # Yield from a generator that zips all the inner generators together without padding, until all inner generators have terminated.
+        while True:
+            try:
+                raw_actions_list = next(itertools.zip_longest(*inner_generators))
+                yield tuple(
+                    filter(
+                        lambda x: x is not None,
+                        itertools.chain.from_iterable(raw_actions_list),
+                    )
+                )
+            except StopIteration:
+                return
+
     def __repr__(self):
         items = itertools.chain(
             (repr(arg) for arg in self._args),

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 <https://pyoscx.github.io/scenariogeneration/xosc/scenario.html>`_ 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

src/scenic/core/simulators.py

@@ -489,6 +489,8 @@ def _run(self, dynamicScenario, maxSteps):
 
                 # Run the agent's behavior to get its actions
                 actions = agent.behavior._step()
+                if actions is None:
+                    actions = tuple()
 
                 # Handle pseudo-actions marking the end of a simulation/scenario
                 if isinstance(actions, _EndSimulationAction):
@@ -811,7 +813,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):