Adding objects that can replay completed scenarios

src/scenic/domains/driving/simulators.py

@@ -59,3 +59,4 @@ 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
+

src/scenic/simulators/carla/simulator.py

@@ -16,9 +16,10 @@
 import pygame
 
 from scenic.domains.driving.simulators import DrivingSimulator, DrivingSimulation
-from scenic.core.simulators import SimulationCreationError
+from scenic.core.simulators import SimulationCreationError, ReplaySimulation
 from scenic.syntax.veneer import verbosePrint
 from scenic.simulators.carla.blueprints import oldBlueprintNames
+from scenic.domains.driving.controllers import PIDLongitudinalController, PIDLateralController
 import scenic.simulators.carla.utils.utils as utils
 import scenic.simulators.carla.utils.visuals as visuals
 
@@ -272,3 +273,66 @@ def destroy(self):
 
 		self.world.tick()
 		super().destroy()
+
+def getLaneFollowingControllers(timestep, isCar):
+	dt = timestep
+	if isCar:
+		lon_controller = PIDLongitudinalController(K_P=0.5, K_D=0.1, K_I=0.7, dt=dt)
+		lat_controller = PIDLateralController(K_P=0.2, K_D=0.1, K_I=0.0, dt=dt)
+	else:
+		lon_controller = PIDLongitudinalController(K_P=0.25, K_D=0.025, K_I=0.0, dt=dt)
+		lat_controller = PIDLateralController(K_P=0.2, K_D=0.1, K_I=0.0, dt=dt)
+	return lon_controller, lat_controller
+
+
+def getTurningControllers(timestep, isCar):
+	"""Get longitudinal and lateral controllers for turning."""
+	dt = timestep
+	if isCar:
+		lon_controller = PIDLongitudinalController(K_P=0.5, K_D=0.1, K_I=0.7, dt=dt)
+		lat_controller = PIDLateralController(K_P=0.8, K_D=0.2, K_I=0.0, dt=dt)
+	else:
+		lon_controller = PIDLongitudinalController(K_P=0.25, K_D=0.025, K_I=0.0, dt=dt)
+		lat_controller = PIDLateralController(K_P=0.4, K_D=0.1, K_I=0.0, dt=dt)
+	return lon_controller, lat_controller
+
+
+def getLaneChangingControllers(timestep, isCar):
+	"""Get longitudinal and lateral controllers for lane changing."""
+	dt = timestep
+	if isCar:
+		lon_controller = PIDLongitudinalController(K_P=0.5, K_D=0.1, K_I=0.7, dt=dt)
+		lat_controller = PIDLateralController(K_P=0.08, K_D=0.3, K_I=0.0, dt=dt)
+	else:
+		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 CarlaReplaySimulation(ReplaySimulation):
+
+	def __init__(self, scene, simulationResult, verbosity=0):
+		super().__init__(scene, simulationResult, verbosity=verbosity,
+						 actionComparisonMethod=self.compareSimulatorActions)
+
+	def compareSimulatorActions(self, action, otherAction):
+		# actions in this simulator should have (steer, throttle) values
+		difference = 0
+		if hasattr(action, "steer") and hasattr(action, "throttle") \
+			and hasattr(otherAction, "steer") and hasattr(otherAction, "throttle"):
+			# get the root mse difference
+			difference = (action.steer - otherAction.steer) ** 2 + \
+						(action.throttle - otherAction.throttle) ** 2 + \
+						(action.brake - otherAction.brake) ** 2
+		else:
+			if action != otherAction:
+				difference += 1
+		return difference
+
+	def getLaneFollowingControllers(self, agent):
+		return getLaneFollowingControllers(self.timestep, agent.isCar)
+
+	def getTurningControllers(self, agent):
+		return getTurningControllers(self.timestep, agent.isCar)
+
+	def getLaneChangingControllers(self, agent):
+		return getLaneChangingControllers(self.timestep, agent.isCar)

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

@@ -10,7 +10,7 @@
 from scenic.domains.driving.simulators import DrivingSimulator, DrivingSimulation
 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
@@ -211,31 +211,71 @@ def getProperties(self, obj, properties):
         return values
 
     def getLaneFollowingControllers(self, agent):
-        dt = self.timestep
-        if agent.isCar:
-            lon_controller = PIDLongitudinalController(K_P=0.5, K_D=0.1, K_I=0.7, dt=dt)
-            lat_controller = PIDLateralController(K_P=0.1, K_D=0.1, K_I=0.02, dt=dt)
-        else:
-            lon_controller = PIDLongitudinalController(K_P=0.25, K_D=0.025, K_I=0.0, dt=dt)
-            lat_controller = PIDLateralController(K_P=0.2, K_D=0.1, K_I=0.0, dt=dt)
-        return lon_controller, lat_controller
+        return getLaneFollowingControllers(self.timestep, agent.isCar)
 
     def getTurningControllers(self, agent):
-        dt = self.timestep
-        if agent.isCar:
-            lon_controller = PIDLongitudinalController(K_P=0.5, K_D=0.1, K_I=0.7, dt=dt)
-            lat_controller = PIDLateralController(K_P=0.2, K_D=0.2, K_I=0.2, dt=dt)
-        else:
-            lon_controller = PIDLongitudinalController(K_P=0.25, K_D=0.025, K_I=0.0, dt=dt)
-            lat_controller = PIDLateralController(K_P=0.4, K_D=0.1, K_I=0.0, dt=dt)
-        return lon_controller, lat_controller
+        return getTurningControllers(self.timestep, agent.isCar)
 
     def getLaneChangingControllers(self, agent):
-        dt = self.timestep
-        if agent.isCar:
-            lon_controller = PIDLongitudinalController(K_P=0.5, K_D=0.1, K_I=0.7, dt=dt)
-            lat_controller = PIDLateralController(K_P=0.2, K_D=0.2, K_I=0.02, dt=dt)
+        return getLaneChangingControllers(self.timestep, agent.isCar)
+
+def getLaneFollowingControllers(timestep, isCar):
+    dt = timestep
+    if isCar:
+        lon_controller = PIDLongitudinalController(K_P=0.5, K_D=0.1, K_I=0.7, dt=dt)
+        lat_controller = PIDLateralController(K_P=0.1, K_D=0.1, K_I=0.02, dt=dt)
+    else:
+        lon_controller = PIDLongitudinalController(K_P=0.25, K_D=0.025, K_I=0.0, dt=dt)
+        lat_controller = PIDLateralController(K_P=0.2, K_D=0.1, K_I=0.0, dt=dt)
+    return lon_controller, lat_controller
+
+def getTurningControllers(timestep, isCar):
+    dt = timestep
+    if isCar:
+        lon_controller = PIDLongitudinalController(K_P=0.5, K_D=0.1, K_I=0.7, dt=dt)
+        lat_controller = PIDLateralController(K_P=0.2, K_D=0.2, K_I=0.2, dt=dt)
+    else:
+        lon_controller = PIDLongitudinalController(K_P=0.25, K_D=0.025, K_I=0.0, dt=dt)
+        lat_controller = PIDLateralController(K_P=0.4, K_D=0.1, K_I=0.0, dt=dt)
+    return lon_controller, lat_controller
+
+def getLaneChangingControllers(timestep, isCar):
+    dt = timestep
+    if isCar:
+        lon_controller = PIDLongitudinalController(K_P=0.5, K_D=0.1, K_I=0.7, dt=dt)
+        lat_controller = PIDLateralController(K_P=0.2, K_D=0.2, K_I=0.02, dt=dt)
+    else:
+        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, verbosity=0):
+        super().__init__(scene, simulationResult, verbosity=verbosity,
+                         actionComparisonMethod=self.compareSimulatorActions)
+
+    def compareSimulatorActions(self, action, otherAction):
+        # actions in this simulator are (steer, throttle) values
+        difference = 0
+        if hasattr(action, "steer") and hasattr(action, "throttle") \
+         and hasattr(otherAction, "steer") and hasattr(otherAction, "throttle"):
+            # we'll take MSE, since these values are already normalized
+            difference = (action.steer - otherAction.steer) ** 2 + \
+                   (action.throttle - otherAction.throttle) ** 2 + \
+                   (action.brake - otherAction.brake) ** 2
         else:
-            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
+            if action != otherAction:
+                difference = 1
+        return difference
+
+    def getLaneFollowingControllers(self, agent):
+        return getLaneFollowingControllers(self.timestep, agent.isCar)
+
+    def getTurningControllers(self, agent):
+        return getTurningControllers(self.timestep, agent.isCar)
+
+    def getLaneChangingControllers(self, agent):
+        return getLaneChangingControllers(self.timestep, agent.isCar)
+
+
+

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,26 @@
+"""Tests for the ReplaySimulation Object, pythonically"""
+
+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
+
+def test_saving_simulation(loadLocalScenario):
+    scenario = loadLocalScenario('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)
+    new_scenario = loadLocalScenario('basic_modified.scenic')
+    new_scene, _ = new_scenario.generate(maxIterations=1000)
+    replay_simulation = NewtonianReplaySimulation(new_scene, original_simulation_result, verbosity=5)
+    replay_simulation.run(100)
+    return replay_simulation