[WIP] OpenScenarioXML Export

pyproject.toml

@@ -59,6 +59,9 @@ metadrive = [
 	"metadrive-simulator@git+https://github.com/metadriverse/metadrive.git@85e5dadc6c7436d324348f6e3d8f8e680c06b4db",
 	"sumolib >= 1.21.0",
 ]
+openscenario = [
+	"scenariogeneration"
+]
 test = [	# minimum dependencies for running tests (used for tox virtualenvs)
 	"pytest >= 7.0.0, <9",
 	"pytest-cov >= 3.0.0",
@@ -68,6 +71,7 @@ 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[metadrive]",
+	"scenic[openscenario]",
 	"astor >= 0.8.1",
 	'carla >= 0.9.12; python_version <= "3.10" and (platform_system == "Linux" or platform_system == "Windows")',
 	"dill",

src/scenic/core/serialization.py

@@ -7,12 +7,15 @@
 
 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
 
 ## JSON
 
@@ -364,3 +367,204 @@ def readStr(stream):
 
 
 Serializer.addCodec(str, writeStr, readStr)
+
+from scenariogeneration import ScenarioGenerator, xosc
+
+
+def toOpenScenario(
+    scenario,
+    scene,
+    simulationResult,
+    mapPath=None,
+    scenarioName="ScenicScenario",
+    wheelbaseRatio=0.6,
+    maxSteeringAngle=0.523598775598,
+    wheelDiameter=0.8,
+    trackWidth=1.68,
+    groundClearance=0.4,
+    maxSpeed=69,
+    maxAcceleration=10,
+    maxDeceleration=10,
+    pedestrianMass=65,
+):
+    # Create catalog
+    xosc_catalog = xosc.Catalog()
+
+    # Create parameters
+    xosc_paramdec = xosc.ParameterDeclarations()
+
+    # Extract map
+    assert "map" in scenario.params
+    map_path = mapPath if mapPath is not None else os.path.abspath(scenario.params["map"])
+    xosc_road = xosc.RoadNetwork(roadfile=map_path)
+
+    # 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(
+                maxSteeringAngle,
+                wheelDiameter,
+                trackWidth,
+                wheelbaseRatio * obj.length,
+                groundClearance,
+            )
+            veh_ra = xosc.Axle(
+                maxSteeringAngle, wheelDiameter, trackWidth, 0, groundClearance
+            )
+            xosc_obj = xosc.Vehicle(
+                name=obj_name,
+                vehicle_type=xosc.VehicleCategory.car,
+                boundingbox=veh_bb,
+                frontaxle=veh_fa,
+                rearaxle=veh_ra,
+                max_speed=maxSpeed,
+                max_acceleration=maxAcceleration,
+                max_deceleration=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=pedestrianMass,
+                boundingbox=ped_bb,
+                category=xosc.PedestrianCategory.pedestrian,
+                model=None,
+                role=None,
+            )
+        else:
+            warnings.warn(f"Unknown object {obj} is ignored.")
+            continue
+
+        xosc_objects[obj] = xosc_obj
+        entities.add_scenario_object(obj_name, xosc_obj)
+
+    # Create init
+    init = xosc.Init()
+
+    for obj, xosc_obj in xosc_objects.items():
+        scenic_yaw = obj.yaw
+        state_orientation = scenic_yaw + math.radians(90)
+        state_position = obj.position.offsetRotated(
+            scenic_yaw, Vector(0, -0.5 * wheelbaseRatio * obj.length, 0)
+        )
+        init_position = xosc.WorldPosition(
+            x=state_position.x,
+            y=state_position.y,
+            z=state_position.z,
+            h=state_orientation,
+        )
+        obj_init_action = xosc.TeleportAction(init_position)
+        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):
+            scenic_yaw = states.orientations[obj_i].yaw
+            state_orientation = scenic_yaw + math.radians(90)
+            state_position = states.positions[obj_i].offsetRotated(
+                scenic_yaw, Vector(0, -0.5 * wheelbaseRatio * obj.length, 0)
+            )
+            action_times.append(simulationResult.timestep * t)
+            pos = xosc.WorldPosition(
+                x=state_position.x,
+                y=state_position.y,
+                z=state_position.z,
+                h=state_orientation,
+            )
+            action_positions.append(pos)
+
+        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

@@ -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):

src/scenic/domains/driving/model.scenic

@@ -133,6 +133,10 @@ class DrivingObject:
     def isVehicle(self):
         return False
 
+    @property
+    def isPedestrian(self):
+        return False
+
     @property
     def isCar(self):
         return False
@@ -325,6 +329,10 @@ class Pedestrian(DrivingObject):
     length: 0.75
     color: [0, 0.5, 1]
 
+    @property
+    def isPedestrian(self):
+        return True
+
 ## Utility functions
 
 def withinDistanceToAnyCars(car, thresholdDistance):