Driving Domain 3D Compatibility (#254)

* Updated driving domain for optional 3D mode and added tests.

* Updated docs.

* Remove old util function.

* Reverted unintended example change.

* Another example revert.

* Adjust comment.

* Updated error message.

* Another error typo fix.

* Test fixes/cleanup.

* Fixed bad parameter.

* fix pickling of some scenarios in 2D mode

* Pr fixes.

* Apply suggestions from code review

Co-authored-by: Daniel Fremont <dfremont@ucsc.edu>

* Fixed broken test.

---------

Co-authored-by: Daniel Fremont <dfremont@ucsc.edu>

src/scenic/core/object_types.py

@@ -1770,11 +1770,10 @@ def __init_subclass__(cls):
             cls._props_transformed = str(cls)
 
             props = cls._scenic_properties
-            # Raise error if parentOrientation already defined
-            if "parentOrientation" in props:
+            # Raise error if parentOrientation and heading already defined
+            if "parentOrientation" in props and "heading" in props:
                 raise RuntimeError(
-                    "this scenario cannot be run with the --2d flag (the "
-                    f'{cls.__name__} class defines "parentOrientation")'
+                    f'{cls.__name__} defines both "parentOrientation" and "heading"'
                 )
 
             # Map certain properties to their 3D analog

src/scenic/core/scenarios.py

@@ -533,6 +533,10 @@ def generateDefaultRequirements(self):
         for obj in filter(
             lambda x: x.requireVisible and x is not self.egoObject, self.objects
         ):
+            if not self.egoObject:
+                raise InvalidScenarioError(
+                    "requireVisible set to true but no ego is defined"
+                )
             requirements.append(VisibilityRequirement(self.egoObject, obj, self.objects))
 
         return tuple(requirements)

src/scenic/core/serialization.py

@@ -9,6 +9,7 @@
 import math
 import pickle
 import struct
+import types
 
 from scenic.core.distributions import Samplable, needsSampling
 from scenic.core.utils import DefaultIdentityDict
@@ -41,6 +42,30 @@ def dumpAsScenicCode(value, stream):
         stream.write(repr(value))
 
 
+## Pickles
+
+# If dill is installed, register some custom handlers to improve the pickling
+# of Scene and Scenario objects.
+
+try:
+    import dill
+except Exception:
+    pass
+else:
+    _orig_save_module = dill.Pickler.dispatch[types.ModuleType]
+
+    @dill.register(types.ModuleType)
+    def patched_save_module(pickler, obj):
+        # Save Scenic's internal modules by reference to avoid inconsistent versions
+        # as well as some unpicklable objects (and shrink the size of pickles while
+        # we're at it).
+        name = obj.__name__
+        if name == "scenic" or name.startswith("scenic."):
+            pickler.save_reduce(dill._dill._import_module, (name,), obj=obj)
+            return
+        _orig_save_module(pickler, obj)
+
+
 ## Binary serialization format
 
 

src/scenic/domains/driving/model.scenic

@@ -17,6 +17,14 @@ If you are writing a generic scenario that supports multiple maps, you may leave
 ``map`` parameter undefined; then running the scenario will produce an error unless the
 user uses the :option:`--param` command-line option to specify the map.
 
+The ``use2DMap`` global parameter determines whether or not maps are generated in 2D. Currently
+3D maps are not supported, but are under development. By default, this parameter is `False`
+(so that future versions of Scenic will automatically use 3D maps), unless
+:ref:`2D compatibility mode` is enabled, in which case the default is `True`. The parameter
+can be manually set to `True` to ensure 2D maps are used even if the scenario is not compiled
+in 2D compatibility mode.
+
+
 .. note::
 
     If you are using a simulator, you may have to also define simulator-specific global
@@ -38,6 +46,22 @@ from scenic.domains.driving.behaviors import *
 from scenic.core.distributions import RejectionException
 from scenic.simulators.utils.colors import Color
 
+## 2D mode flag & checks
+
+def is2DMode():
+    from scenic.syntax.veneer import mode2D
+    return mode2D
+
+param use2DMap = True if is2DMode() else False
+
+if is2DMode() and not globalParameters.use2DMap:
+    raise RuntimeError('in 2D mode, global parameter "use2DMap" must be True')
+
+# Note: The following should be removed when 3D maps are supported
+if not globalParameters.use2DMap:
+    raise RuntimeError('3D maps not supported at this time.'
+        '(to use 2D maps set global parameter "use2DMap" to True)')
+
 ## Load map and set up workspace
 
 if 'map' not in globalParameters:
@@ -80,10 +104,6 @@ roadDirection : VectorField = network.roadDirection
 
 ## Standard object types
 
-def is2DMode():
-    from scenic.syntax.veneer import mode2D
-    return mode2D
-
 class DrivingObject:
     """Abstract class for objects in a road network.
 
@@ -250,10 +270,10 @@ class Vehicle(DrivingObject):
 
     Properties:
         position: The default position is uniformly random over the `road`.
-        heading: The default heading is aligned with `roadDirection`, plus an offset
+        parentOrientation: The default parentOrientation is aligned with `roadDirection`, plus an offset
             given by **roadDeviation**.
         roadDeviation (float): Relative heading with respect to the road direction at
-            the `Vehicle`'s position. Used by the default value for **heading**.
+            the `Vehicle`'s position. Used by the default value for **parentOrientation**.
         regionContainedIn: The default container is :obj:`roadOrShoulder`.
         viewAngle: The default view angle is 90 degrees.
         width: The default width is 2 meters.
@@ -264,7 +284,7 @@ class Vehicle(DrivingObject):
     """
     regionContainedIn: roadOrShoulder
     position: new Point on road
-    heading: (roadDirection at self.position) + self.roadDeviation
+    parentOrientation: (roadDirection at self.position) + self.roadDeviation
     roadDeviation: 0
     viewAngle: 90 deg
     width: 2
@@ -290,7 +310,7 @@ class Pedestrian(DrivingObject):
 
     Properties:
         position: The default position is uniformly random over sidewalks and crosswalks.
-        heading: The default heading is uniformly random.
+        parentOrientation: The default parentOrientation has uniformly random yaw.
         viewAngle: The default view angle is 90 degrees.
         width: The default width is 0.75 m.
         length: The default length is 0.75 m.
@@ -299,7 +319,7 @@ class Pedestrian(DrivingObject):
     """
     regionContainedIn: network.walkableRegion
     position: new Point on network.walkableRegion
-    heading: Range(0, 360) deg
+    parentOrientation: Range(0, 360) deg
     viewAngle: 90 deg
     width: 0.75
     length: 0.75

src/scenic/simulators/carla/model.scenic

@@ -18,6 +18,8 @@ Global Parameters:
     timestep (float): Timestep to use for simulations (i.e., how frequently Scenic
         interrupts CARLA to run behaviors, check requirements, etc.), in seconds. Default
         is 0.1 seconds.
+    snapToGroundDefault (bool): Default value for :prop:`snapToGround` on `CarlaActor` objects.
+        Default is True if :ref:`2D compatibility mode` is enabled and False otherwise. 
 
     weather (str or dict): Weather to use for the simulation. Can be either a
         string identifying one of the CARLA weather presets (e.g. 'ClearSunset') or a
@@ -96,6 +98,7 @@ param weather = Uniform(
     'MidRainSunset',
     'HardRainSunset'
 )
+param snapToGroundDefault = is2DMode()
 
 simulator CarlaSimulator(
     carla_map=globalParameters.carla_map,
@@ -118,12 +121,15 @@ class CarlaActor(DrivingObject):
         rolename (str): Can be used to differentiate specific actors during runtime. Default
             value ``None``.
         physics (bool): Whether physics is enabled for this object in CARLA. Default true.
+        snapToGround (bool): Whether or not to snap this object to the ground when placed in CARLA.
+            The default is set by the ``snapToGroundDefault`` global parameter above.
     """
     carlaActor: None
     blueprint: None
     rolename: None
     color: None
     physics: True
+    snapToGround: globalParameters.snapToGroundDefault
 
     def __init__(self, *args, **kwargs):
         super().__init__(*args, **kwargs)
@@ -222,12 +228,12 @@ class Prop(CarlaActor):
     """Abstract class for props, i.e. non-moving objects.
 
     Properties:
-        heading (float): Default value overridden to be uniformly random.
+        parentOrientation (Orientation): Default value overridden to have uniformly random yaw.
         physics (bool): Default value overridden to be false.
     """
     regionContainedIn: road
     position: new Point on road
-    heading: Range(0, 360) deg
+    parentOrientation: Range(0, 360) deg
     width: 0.5
     length: 0.5
     physics: False

src/scenic/simulators/carla/simulator.py

@@ -205,7 +205,10 @@ def createObjectInSimulator(self, obj):
 
         # Set up transform
         loc = utils.scenicToCarlaLocation(
-            obj.position, world=self.world, blueprint=obj.blueprint
+            obj.position,
+            world=self.world,
+            blueprint=obj.blueprint,
+            snapToGround=obj.snapToGround,
         )
         rot = utils.scenicToCarlaRotation(obj.orientation)
         transform = carla.Transform(loc, rot)

src/scenic/simulators/carla/utils/utils.py

@@ -7,7 +7,7 @@
 from scenic.core.vectors import Orientation, Vector
 
 
-def snapToGround(world, location, blueprint):
+def _snapToGround(world, location, blueprint):
     """Mutates @location to have the same z-coordinate as the nearest waypoint in @world."""
     waypoint = world.get_map().get_waypoint(location)
     # patch to avoid the spawn error issue with vehicles and walkers.
@@ -25,11 +25,11 @@ def scenicToCarlaVector3D(x, y, z=0.0):
     return carla.Vector3D(x, -y, z)
 
 
-def scenicToCarlaLocation(pos, z=None, world=None, blueprint=None):
-    if z is None:
+def scenicToCarlaLocation(pos, world=None, blueprint=None, snapToGround=False):
+    if snapToGround:
         assert world is not None
-        return snapToGround(world, carla.Location(pos.x, -pos.y, 0.0), blueprint)
-    return carla.Location(pos.x, -pos.y, z)
+        return _snapToGround(world, carla.Location(pos.x, -pos.y, 0.0), blueprint)
+    return carla.Location(pos.x, -pos.y, pos.z)
 
 
 def scenicToCarlaRotation(orientation):
@@ -40,13 +40,6 @@ def scenicToCarlaRotation(orientation):
     return carla.Rotation(pitch=pitch, yaw=yaw, roll=roll)
 
 
-def scenicSpeedToCarlaVelocity(speed, heading):
-    currYaw = scenicToCarlaRotation(heading).yaw
-    xVel = speed * math.cos(currYaw)
-    yVel = speed * math.sin(currYaw)
-    return scenicToCarlaVector3D(xVel, yVel)
-
-
 def carlaToScenicPosition(loc):
     return Vector(loc.x, -loc.y, loc.z)
 

tests/core/test_pickle.py

@@ -1,5 +1,8 @@
+import sys
+
 import pytest
 
+import scenic
 from scenic.core.distributions import (
     Normal,
     Options,
@@ -95,6 +98,22 @@ def test_pickle_scene():
     tryPickling(scene)
 
 
+def test_pickle_scenario_2D_module():
+    """Tests a nasty bug involving pickling the scenic module in 2D mode."""
+    scenario = compileScenic(
+        """
+        import scenic
+        ego = new Object with favoriteModule scenic
+        """,
+        mode2D=True,
+    )
+    sc = tryPickling(scenario)
+    assert sys.modules["scenic.core.object_types"].Object is Object
+    ego = sampleEgo(sc)
+    assert isinstance(ego, Object)
+    assert ego.favoriteModule is scenic
+
+
 def test_pickle_scenario_dynamic():
     scenario = compileScenic(
         """

tests/domains/driving/test_driving.py

@@ -33,13 +33,32 @@
 from tests.domains.driving.conftest import map_params, mapFolder
 
 
-def compileDrivingScenario(cached_maps, code="", useCache=True, path=None):
+def compileDrivingScenario(
+    cached_maps, code="", useCache=True, path=None, mode2D=True, params={}
+):
     if not path:
         path = mapFolder / "CARLA" / "Town01.xodr"
     path = cached_maps[str(path)]
     preamble = template.format(map=path, cache=useCache)
     whole = preamble + "\n" + inspect.cleandoc(code)
-    return compileScenic(whole, mode2D=True)
+    return compileScenic(whole, mode2D=mode2D, params=params)
+
+
+def test_driving_2D_map(cached_maps):
+    compileDrivingScenario(
+        cached_maps,
+        code=basicScenario,
+        useCache=False,
+        mode2D=False,
+        params={"use2DMap": True},
+    )
+
+
+def test_driving_3D(cached_maps):
+    with pytest.raises(RuntimeError):
+        compileDrivingScenario(
+            cached_maps, code=basicScenario, useCache=False, mode2D=False
+        )
 
 
 @pytest.mark.slow

tests/simulators/newtonian/test_newtonian.py

@@ -21,7 +21,7 @@ def test_render(loadLocalScenario):
     simulator.simulate(scene, maxSteps=3)
 
 
-def test_driving(loadLocalScenario):
+def test_driving_2D(loadLocalScenario):
     def check():
         scenario = loadLocalScenario("driving.scenic", mode2D=True)
         scene, _ = scenario.generate(maxIterations=1000)

tests/syntax/test_basic.py

@@ -13,7 +13,13 @@
     setDebuggingOptions,
 )
 from scenic.core.object_types import Object
-from tests.utils import compileScenic, sampleEgo, sampleParamPFrom, sampleScene
+from tests.utils import (
+    compileScenic,
+    sampleEgo,
+    sampleEgoFrom,
+    sampleParamPFrom,
+    sampleScene,
+)
 
 
 def test_minimal():
@@ -296,3 +302,31 @@ def test_mode2D_interference():
         scene, _ = scenario.generate()
 
         assert any(obj.position[2] != 0 for obj in scene.objects)
+
+
+def test_mode2D_heading_parentOrientation():
+    program = """
+            class Foo:
+                heading: 0.56
+
+            class Bar(Foo):
+                parentOrientation: 1.2
+
+            ego = new Bar
+        """
+
+    obj = sampleEgoFrom(program, mode2D=True)
+    assert obj.heading == obj.parentOrientation.yaw == 1.2
+
+    program = """
+            class Bar:
+                parentOrientation: 1.2
+
+            class Foo(Bar):
+                heading: 0.56
+
+            ego = new Foo
+        """
+
+    obj = sampleEgoFrom(program, mode2D=True)
+    assert obj.heading == obj.parentOrientation.yaw == 0.56

tests/syntax/test_specifiers.py

@@ -555,6 +555,11 @@ def test_visible_no_ego():
         compileScenic("ego = new Object visible")
 
 
+def test_visible_no_ego_2():
+    with pytest.raises(InvalidScenarioError):
+        compileScenic("new Object visible")
+
+
 def test_visible_from_point():
     scenario = compileScenic(
         "x = new Point at 300@200, with visibleDistance 2\n"