Adding objects that can replay completed scenarios

src/scenic/core/simulators.py

@@ -213,6 +213,7 @@ def run(self, maxSteps):
                     if not self.actionsAreCompatible(agent, actions):
                         raise InvalidScenarioError(f'agent {agent} tried incompatible '
                                                    f' action(s) {actions}')
+                    # to make saveable, we record agents by their properties
                     allActions[agent] = actions
                 if terminationReason is not None:
                     break
@@ -353,9 +354,9 @@ def currentState(self):
         The definition of 'state' is up to the simulator; the 'state' is simply saved
         at each time step to define the 'trajectory' of the simulation.
 
-        The default implementation returns a tuple of the positions of all objects.
+        The default implementation a list of objects with their properties.
         """
-        return tuple(obj.position for obj in self.objects)
+        return [self.getProperties(obj, obj.properties) for obj in self.objects]
 
     @property
     def currentRealTime(self):
@@ -365,6 +366,190 @@ def destroy(self):
         """Perform any cleanup necessary to reset the simulator after a simulation."""
         pass
 
+class ReplaySimulation(Simulation):
+    """An object that reconstitutes a completed single simulation and allows for
+    re-stepping through the simulation, along with the ability to compare
+    behaviors that were not originally used in simulation to the states and actions
+    that were recorded as the result of the simulation.
+
+    Attributes:
+        currentTime (int): Number of time steps elapsed so far.
+        timestep (float): Length of each time step in seconds.
+        objects: List of Scenic objects (instances of `Object`) existing in the
+            simulation. This list will change if objects are created dynamically.
+        agents: List of :term:`agents` in the simulation.
+        simulationResult (`SimulationResult`):
+    """
+
+    def __init__(self, scene, simulationResult, timestep=1, verbosity=0):
+        super().__init__(scene, timestep=timestep, verbosity=verbosity)
+        self.scene = scene
+        self.simulation_result = simulationResult
+        self.objects = list(scene.objects)
+        self.agents = list(obj for obj in scene.objects if obj.behavior is not None)
+        self.trajectory = self.simulation_result.trajectory
+        self.action_differences = []
+        self.records = defaultdict(list)
+        self.currentTime = 0
+        self.timestep = timestep
+        self.verbosity = verbosity
+        self.worker_num = 0
+
+    def check_consistency(self):
+        # check that objects in scene and objects in the simulationResult are initially the same
+        for idx, obj in enumerate(self.objects):
+            # get the corresponding object dict from the result's initial state
+            result_obj_dict = self.simulation_result.initial_state[idx]
+            assert self.getProperties(obj, obj.properties) == result_obj_dict
+
+    def updateObjects(self):
+        """Update the positions and other properties of objects from the simulation."""
+        current_replay_objects = self.simulation_result.trajectory[self.currentTime]
+        """Update the positions and other properties of objects from the simulation."""
+        for idx, obj in enumerate(self.objects):
+            # Get latest values of dynamic properties from simulation
+            properties = obj.properties
+            values = self.getProperties(current_replay_objects[idx], obj.properties)
+            assert properties == set(values), properties ^ set(values)
+
+            # Preserve some other properties which are assigned internally by Scenic
+            # TODO: do we still need this?
+            # for prop in self.mutableProperties(obj):
+            #     values[prop] = getattr(obj, prop)
+
+            # Make a new copy of the object to ensure that computed properties like
+            # visibleRegion, etc. are recomputed
+            setDynamicProxyFor(obj, obj._copyWith(**values))
+
+    def reset_replay(self):
+        self.currentTime = 0
+        self.action_differences = []
+
+    def run(self, maxSteps):
+        """Run the simulation.
+
+        Throws a RejectSimulationException if a requirement is violated.
+        """
+        if self.currentTime > 0:
+            raise RuntimeError('tried to replay a Simulation which has already been replayed')
+        actionSequence = []
+
+        import scenic.syntax.veneer as veneer
+        veneer.beginSimulation(self)
+        dynamicScenario = self.scene.dynamicScenario
+
+        try:
+
+            # Give objects a chance to do any simulator-specific setup
+            # TODO: do we still need this?
+            for obj in self.objects:
+                obj.startDynamicSimulation()
+
+            # Update all objects to the initial state
+            self.updateObjects()
+
+            # Run simulation
+            assert self.currentTime == 0
+            while True:
+                if self.verbosity >= 3:
+                    print(f'    Time step {self.currentTime}:')
+
+                # Don't record the current state since we're replaying
+                # an existing recording
+
+                # Run monitors
+                newReason = dynamicScenario._runMonitors()
+                if newReason is not None:
+                    terminationReason = newReason
+                    terminationType = TerminationType.terminatedByMonitor
+
+                # "Always" and scenario-level requirements have been checked;
+                # now safe to terminate if the top-level scenario has finished,
+                # a monitor requested termination, or we've hit the timeout
+                if terminationReason is not None:
+                    break
+                terminationReason = dynamicScenario._checkSimulationTerminationConditions()
+                if terminationReason is not None:
+                    terminationType = TerminationType.simulationTerminationCondition
+                    break
+                if maxSteps and self.currentTime >= maxSteps:
+                    terminationReason = f'reached time limit ({maxSteps} steps)'
+                    terminationType = TerminationType.timeLimit
+                    break
+                # Compute the actions of the agents in this time step
+                allActions = OrderedDict()
+                schedule = self.scheduleForAgents()
+                for agent in schedule:
+                    behavior = agent.behavior
+                    if not behavior._runningIterator:   # TODO remove hack
+                        behavior._start(agent)
+                    actions = behavior._step()
+                    if isinstance(actions, EndSimulationAction):
+                        terminationReason = str(actions)
+                        terminationType = TerminationType.terminatedByBehavior
+                        break
+                    assert isinstance(actions, tuple)
+                    if len(actions) == 1 and isinstance(actions[0], (list, tuple)):
+                        actions = tuple(actions[0])
+                    if not self.actionsAreCompatible(agent, actions):
+                        raise InvalidScenarioError(f'agent {agent} tried incompatible '
+                                                   f' action(s) {actions}')
+                    allActions[agent] = actions
+                if terminationReason is not None:
+                    break
+                # Execute the actions
+                if self.verbosity >= 3:
+                    for agent, actions in allActions.items():
+                        print(f'      Agent {agent} takes action(s) {actions}')
+                actionSequence.append(allActions)
+                self.action_differences.append(self.compareActions(allActions))
+
+                self.currentTime += 1
+                # After incrementing time, read the next saved simulation state back into Scenic
+                self.updateObjects()
+
+            # Stop all remaining scenarios
+            # (and reject if some 'require eventually' condition was never satisfied)
+            for scenario in tuple(veneer.runningScenarios):
+                scenario._stop('simulation terminated')
+
+            # Record finally-recorded values
+            values = dynamicScenario._evaluateRecordedExprs(RequirementType.recordFinal)
+            for name, val in values.items():
+                self.records[name] = val
+
+            return self
+        finally:
+            self.destroy()
+            for obj in self.scene.objects:
+                disableDynamicProxyFor(obj)
+            for agent in self.agents:
+                if agent.behavior._isRunning:
+                    agent.behavior._stop()
+            for monitor in self.scene.monitors:
+                if monitor._isRunning:
+                    monitor._stop()
+            # If the simulation was terminated by an exception (including rejections),
+            # some scenarios may still be running; we need to clean them up without
+            # checking their requirements, which could raise rejection exceptions.
+            for scenario in tuple(veneer.runningScenarios):
+                scenario._stop('exception', quiet=True)
+            veneer.endSimulation(self)
+
+    def compareActions(self, allActions):
+        action_differences = OrderedDict()
+        recorded_action_dict = self.simulation_result.actions[self.currentTime]
+        for obj_idx, (obj_dict, action) in enumerate(recorded_action_dict.items()):
+            action_differences[obj_dict] = self.compareSimulatorActions(action, allActions[obj_idx])
+        return action_differences
+
+    def compareSimulatorActions(self, action, otherAction):
+        """ Simulator-specific comparison for actions."""
+        raise NotImplementedError
+
+    def executeActions(self, allActions):
+        pass
+
 class DummySimulator(Simulator):
     """Simulator which does nothing, for debugging purposes."""
     def __init__(self, timestep=1):
@@ -458,6 +643,7 @@ class SimulationResult:
     def __init__(self, trajectory, actions, terminationType, terminationReason, records):
         self.trajectory = tuple(trajectory)
         assert self.trajectory
+        self.initial_state = self.trajectory[0]
         self.finalState = self.trajectory[-1]
         self.actions = tuple(actions)
         self.terminationType = terminationType

src/scenic/simulators/newtonian/__init__.py

@@ -14,4 +14,4 @@
    simulator
 """
 
-from .simulator import NewtonianSimulator
+from .simulator import NewtonianSimulator, NewtonianReplaySimulation

src/scenic/simulators/newtonian/simulator.py

@@ -8,9 +8,10 @@
 import time
 
 from scenic.domains.driving.simulators import DrivingSimulator, DrivingSimulation
+from scenic.domains.driving.actions import RegulatedControlAction
 from scenic.core.geometry import allChains
 from scenic.core.regions import toPolygon
-from scenic.core.simulators import SimulationCreationError
+from scenic.core.simulators import SimulationCreationError, ReplaySimulation
 from scenic.syntax.veneer import verbosePrint
 from scenic.core.vectors import Vector
 import scenic.simulators.newtonian.utils.utils as utils
@@ -239,3 +240,22 @@ def getLaneChangingControllers(self, agent):
             lon_controller = PIDLongitudinalController(K_P=0.25, K_D=0.025, K_I=0.0, dt=dt)
             lat_controller = PIDLateralController(K_P=0.1, K_D=0.3, K_I=0.0, dt=dt)
         return lon_controller, lat_controller
+
+class NewtonianReplaySimulation(ReplaySimulation):
+    def __init__(self, scene, simulationResult, timestep=1, verbosity=0):
+        super().__init__(scene, simulationResult, timestep=timestep, verbosity=verbosity)
+
+    def compareSimulatorActions(self, actions, otherActions):
+        # actions in this simulator are (steer, throttle) values
+        difference = 0
+        # we'll take MSE, since these values are already normalized
+        for idx in range(len(actions)):
+            action, otherAction = actions[idx], otherActions[idx]
+            if isinstance(action, RegulatedControlAction) and \
+                isinstance(otherAction, RegulatedControlAction):
+                difference += (action.steer - otherAction.steer) ** 2 + \
+                       (action.throttle - otherAction.throttle) ** 2
+            else:
+                if action != otherAction:
+                    difference += 1
+

tests/simulators/replay/basic.scenic

@@ -0,0 +1,15 @@
+param map = localPath('../../formats/opendrive/maps/CARLA/Town01.xodr')
+model scenic.simulators.newtonian.model
+
+ego = Car in intersection, with behavior FollowLaneBehavior(target_speed=9)
+
+ego = Car on ego.lane.predecessor, with behavior FollowLaneBehavior(target_speed=9)
+
+Pedestrian on visible sidewalk
+
+third = Car on visible ego.road
+require abs((apparent heading of third) - 180 deg) <= 30 deg
+
+Object visible, with width 0.1, with length 0.1
+
+terminate after 6 seconds

tests/simulators/replay/basic_modified.scenic

@@ -0,0 +1,15 @@
+param map = localPath('../../formats/opendrive/maps/CARLA/Town01.xodr')
+model scenic.simulators.newtonian.model
+
+ego = Car in intersection, with behavior FollowLaneBehavior(target_speed=10)
+
+ego = Car on ego.lane.predecessor, with behavior FollowLaneBehavior(target_speed=9)
+
+Pedestrian on visible sidewalk
+
+third = Car on visible ego.road
+require abs((apparent heading of third) - 180 deg) <= 30 deg
+
+Object visible, with width 0.1, with length 0.1
+
+terminate after 6 seconds

tests/simulators/replay/test_replay.py

@@ -0,0 +1,34 @@
+"""Tests for the ReplaySimulation Object, pythonically"""
+
+import inspect
+import os
+import re
+import scenic
+from scenic.simulators.newtonian import NewtonianSimulator, NewtonianReplaySimulation
+import pytest
+import pickle
+
+# Mark all tests in this file as slow, since they require spawning a subprocess
+pytestmark = pytest.mark.slow
+
+## Utilities
+
+paramPattern = re.compile(r'\s*Parameter "p": (.*)$')
+
+def test_replay_simulation():
+    scenario = scenic.scenarioFromFile(os.getcwd() + '/basic.scenic')
+    scene, _ = scenario.generate(maxIterations=1000)
+    simulator = NewtonianSimulator()
+    original_simulation = simulator.simulate(scene, maxSteps=3)
+    original_simulation_result = original_simulation.result
+    # try pickling and saving the result, first
+    # with open("original_simulation_result", 'wb') as fle:
+    #     pickle.dump(original_simulation_result, fle)
+    # now, try loading the simulaton result and new scene into a Replay object
+    new_scenario = scenic.scenarioFromFile(os.getcwd() + '/basic_modified.scenic')
+    new_scene, _ = new_scenario.generate(maxIterations=1000)
+    replay_simulation = NewtonianReplaySimulation(scene, original_simulation_result, verbosity=3)
+    replay_simulation.run(100)
+    return replay_simulation
+
+test_replay_simulation()