diff --git a/pyproject.toml b/pyproject.toml index b1ade058c..cc988206a 100644 --- a/pyproject.toml +++ b/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", diff --git a/src/scenic/core/serialization.py b/src/scenic/core/serialization.py index a7c52367a..bba11549d 100644 --- a/src/scenic/core/serialization.py +++ b/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 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 b6d0754af..4a3219a56 100644 --- a/src/scenic/domains/driving/model.scenic +++ b/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):