diff --git a/.gitignore b/.gitignore index aa502d3e5..c1288f49a 100644 --- a/.gitignore +++ b/.gitignore @@ -137,8 +137,9 @@ dmypy.json *.cproject -# generated parser +# generated parser and checksum src/scenic/syntax/parser.py +src/scenic/syntax/.parser_checksum # generated media/output simulation.gif diff --git a/docs/api.rst b/docs/api.rst index 440f450f2..0536b30cd 100644 --- a/docs/api.rst +++ b/docs/api.rst @@ -30,8 +30,8 @@ the sampled values for all the global parameters and objects in the scene from t .. testcode:: - import random, scenic - random.seed(12345) + import scenic + scenic.setSeed(12345) scenario = scenic.scenarioFromString('ego = new Object with foo Range(0, 5)') scene, numIterations = scenario.generate() print(f'ego has foo = {scene.egoObject.foo}') @@ -40,6 +40,13 @@ the sampled values for all the global parameters and objects in the scene from t ego has foo = 2.083099362726706 +Utilities +--------- + +Scenic provides top level utility functions for functions like setting Scenic's random seed. + +.. autofunction:: scenic.setSeed + Running Dynamic Simulations --------------------------- @@ -173,6 +180,15 @@ it to the replay, but this greatly increases the size of the encoded simulation. can return to one later for further analysis), but it is not guaranteed to be compatible across major versions of Scenic. +.. _xosc_export: + +OpenScenarioXML Export +---------------------- + +Scenic provides experimental support for exporting completed simulations via `toOpenScenario`. +This function currently only supports cars and pedestrians, and may be subject to breaking changes +in the future. + .. seealso:: If you get exceptions or unexpected behavior when using the API, Scenic provides various debugging features: see :ref:`debugging`. .. rubric:: Footnotes diff --git a/docs/options.rst b/docs/options.rst index 956399c4c..4e97496cc 100644 --- a/docs/options.rst +++ b/docs/options.rst @@ -48,6 +48,8 @@ General Scenario Control (although :mod:`random` and :mod:`numpy.random` should not be used in place of Scenic's own sampling constructs in Scenic code). + The seed can be set programatically using `scenic.setSeed`. + .. option:: --scenario If the given Scenic file defines multiple scenarios, select which one to run. diff --git a/docs/reference/dynamic_scenarios.rst b/docs/reference/dynamic_scenarios.rst index ad4512912..524570350 100644 --- a/docs/reference/dynamic_scenarios.rst +++ b/docs/reference/dynamic_scenarios.rst @@ -33,12 +33,12 @@ In detail, a single time step of a dynamic simulation is executed according to t If the block executes a :keyword:`require` statement with a false condition, reject the simulation. If it executes :keyword:`terminate` or :keyword:`terminate simulation`, or finishes executing, go to step (e) below to stop the scenario. - e. If the scenario is stopping for one of the reasons above, first recursively stop any sub-scenarios it is running, then revert the effects of any :keyword:`override` statements it executed. + e. If the scenario is stopping for one of the reasons above, save the values of any :keyword:`record final` statements in the scenario, recursively stop any sub-scenarios it is running, then revert the effects of any :keyword:`override` statements it executed. Next, check if any of its :term:`temporal requirements` were not satisfied: if so, reject the simulation. Otherwise, the scenario returns to its parent scenario if it was invoked using :keyword:`do`; if it was the top-level scenario, or if it executed :keyword:`terminate simulation`, we set a flag indicating the top-level scenario has terminated. (We do not terminate immediately since we still need to check monitors in the next step.) -2. Save the values of all :keyword:`record` statements, as well as :keyword:`record initial` statements if it is time step 0. +2. Save the values of all :keyword:`record` statements in currently-running scenarios, as well as :keyword:`record initial` statements for scenarios which have just started. 3. Run each :term:`monitor` instantiated in the currently-running scenarios for one time step (i.e. resume it until it executes :keyword:`wait`). If it executes a :keyword:`require` statement with a false condition, reject the simulation. @@ -66,8 +66,7 @@ In detail, a single time step of a dynamic simulation is executed according to t 9. Update every :term:`dynamic property` of every object to its current value in the simulator. -10. If the simulation is stopping for one of the reasons above, first check if any of the :term:`temporal requirements` of any remaining scenarios were not satisfied: if so, reject the simulation. - Otherwise, save the values of any :keyword:`record final` statements. +10. If the simulation is stopping for one of the reasons above, stop any remaining scenarios as in step (1e) above (including checking :term:`temporal requirements` and saving the values of :keyword:`record final` statements). .. rubric:: Footnotes diff --git a/docs/reference/operators.rst b/docs/reference/operators.rst index 4a39539e9..b8fc0f6c6 100644 --- a/docs/reference/operators.rst +++ b/docs/reference/operators.rst @@ -28,6 +28,12 @@ apparent heading of *OrientedPoint* [from *vector*] --------------------------------------------------- The apparent heading of the OrientedPoint, with respect to the line of sight from ego (or the position provided with the optional from vector) +.. _apparent heading to {vector} [from {OrientedPoint}]: + +apparent heading to *vector* [from *OrientedPoint*] +--------------------------------------------------- +The apparent heading to the vector, with respect to the line of sight from ego (or the OrientedPoint provided with the optional from OrientedPoint) + .. _distance [from {vector}] to {vector}: .. _distance from: @@ -35,6 +41,14 @@ distance [from *vector*] to *vector* ------------------------------------- The distance to the given position from ego (or the position provided with the optional from vector) +.. _minimum distance [from {Object}] to {Object}: +.. _minimum distance from: + +minimum distance [from *Object*] to *Object* +-------------------------------------------- +The minimum distance to the given Object from ego (or the Object provided with the optional from Object). Unlike :ref:`distance from`, this operator takes into account the Objects' shapes, sizes, etc... + + .. _angle [from {vector}] to {vector}: angle [from *vector* ] to *vector* @@ -75,6 +89,15 @@ Whether an `Object`/`Region` intersects another `Object`/`Region`, i.e. whether When working with 2D regions, it can be useful to check intersection with the :term:`footprint` of a region, e.g. when checking whether a car intersects a given lane. In this case, one would write :scenic:`car intersects lane.footprint` instead of :scenic:`car intersects lane`. For more details, see :term:`footprint`. +.. _{vector} (ahead of | behind | left of | right of) {region}: + +*vector* (ahead of | behind | left of | right of) *OrientedPoint* +----------------------------------------------------------------- +Whether a vector is to a respective side of an `OrientedPoint` (i.e. within +- ~90 degrees from that direction), accounting for the heading of the `OrientedPoint`. These operators are convenient shorthand for comparing :sampref:`apparent heading to {vector} [from {OrientedPoint}]` to a range of values. + +Note: For vectors very close to a boundary point (i.e. an apparent heading to the vector in (89.99, 90.01) or (-89.99, -90.01) for "ahead of" and "behind"), both operators will resolve to False. + +Example: If an `OrientedPoint` had an apparent heading to a vector of 45 degrees, it would be both "ahead of" and "left of" the `OrientedPoint` but not "behind" or "right of". Orientation Operators ===================== diff --git a/docs/reference/statements.rst b/docs/reference/statements.rst index aa1de02c0..97a8e5c02 100644 --- a/docs/reference/statements.rst +++ b/docs/reference/statements.rst @@ -285,7 +285,7 @@ The default mutation system adds Gaussian noise to the :prop:`position` and :pro record [initial | final] *value* [as *name*] ---------------------------------------------- Record the value of an expression during each simulation. -The value can be recorded at the start of the simulation (``initial``), at the end of the simulation (``final``), or at every time step (if neither ``initial`` nor ``final`` is specified). +The value can be recorded at the start of the scenario (``initial``), at the end of the scenario (``final``), or at every time step during the scenario (if neither ``initial`` nor ``final`` is specified). The recorded values are available in the ``records`` dictionary of `SimulationResult`: its keys are the given names of the records (or synthesized names if not provided), and the corresponding values are either the value of the recorded expression or a tuple giving its value at each time step as appropriate. For debugging, the records can also be printed out using the :option:`--show-records` command-line option. diff --git a/docs/simulators.rst b/docs/simulators.rst index ee5d0b0d1..5a966660b 100644 --- a/docs/simulators.rst +++ b/docs/simulators.rst @@ -14,6 +14,10 @@ See the individual entries for details on each interface's capabilities and how While Scenic aims to support multiple Python versions, some simulators may have more limited compatibility. Be sure to check the documentation of each simulator to confirm which Python versions are supported. +.. note:: + Scenic also supports outputing data in formats that may be imported into other simulators and tools (e.g. :ref:`xosc_export`). + For more details, see :ref:`serialization`. + .. contents:: List of Simulators :local: @@ -163,7 +167,6 @@ This interface is part of the VerifAI toolkit; documentation and examples can be .. _VerifAI repository: https://github.com/BerkeleyLearnVerify/VerifAI - Deprecated ========== diff --git a/pyproject.toml b/pyproject.toml index d94b13276..e5c8fd8a1 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -36,6 +36,7 @@ dependencies = [ "networkx >= 2.6", "numpy >= 1.24", "opencv-python ~= 4.5", + "optuna ~= 4.0", "pegen >= 0.3.0", "pillow >= 9.1", 'pygame-ce >= 2.5.7, <3; python_version >= "3.10"', @@ -60,6 +61,9 @@ metadrive = [ "metadrive-simulator >= 0.4.3", "sumolib >= 1.21.0", ] +openscenario = [ + "scenariogeneration" +] test = [ # minimum dependencies for running tests (used for tox virtualenvs) "pytest >= 7.0.0", "pytest-cov >= 3.0.0", @@ -68,6 +72,7 @@ test = [ # minimum dependencies for running tests (used for tox virtualenvs) 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[openscenario]", "astor >= 0.8.1", 'carla >= 0.9.12; python_version <= "3.12" and (platform_system == "Linux" or platform_system == "Windows")', "dill", diff --git a/src/scenic/__init__.py b/src/scenic/__init__.py index ac15ea073..fe8fbeb72 100644 --- a/src/scenic/__init__.py +++ b/src/scenic/__init__.py @@ -2,6 +2,7 @@ import scenic.core.errors as _errors from scenic.core.errors import setDebuggingOptions +from scenic.core.utils import setSeed from scenic.syntax.translator import scenarioFromFile, scenarioFromString _errors.showInternalBacktrace = False # see comment in errors module diff --git a/src/scenic/__main__.py b/src/scenic/__main__.py index 05f527fba..076ace8c9 100644 --- a/src/scenic/__main__.py +++ b/src/scenic/__main__.py @@ -185,8 +185,7 @@ if args.verbosity >= 1: print(f"Using random seed = {args.seed}") - random.seed(args.seed) - numpy.random.seed(args.seed) + scenic.setSeed(args.seed) # Load scenario from file if args.verbosity >= 1: diff --git a/src/scenic/core/distributions.py b/src/scenic/core/distributions.py index f31165985..c522b56ec 100644 --- a/src/scenic/core/distributions.py +++ b/src/scenic/core/distributions.py @@ -126,6 +126,7 @@ def __init__(self, dependencies): props.update(requiredProperties(dep)) super().__init__(props, deps) self._conditioned = self # version (partially) conditioned on requirements + self._conditionTarget = False @staticmethod def sampleAll(quantities): @@ -172,6 +173,8 @@ def deserializeValue(self, serializer, values): def conditionTo(self, value): """Condition this value to another value with the same conditional distribution.""" + assert not self._conditionTarget, "Attempted to double condition!" + value._conditionTarget = True assert isinstance(value, Samplable) self._conditioned = value @@ -182,6 +185,17 @@ def evaluateIn(self, context): assert all(not needsLazyEvaluation(dep) for dep in value._dependencies) return value + def recursiveDependencies(self): + self = self._conditioned + deps = set( + itertools.chain.from_iterable( + dep.recursiveDependencies() for dep in self._dependencies + ) + ) + if isinstance(self, Distribution) and not self._deterministic: + deps.add(self) + return deps + class ConstantSamplable(Samplable): """A samplable which always evaluates to a constant value. @@ -1076,6 +1090,7 @@ def cdf(mean, stddev, x): return (1 + math.erf((x - mean) / (sqrt2 * stddev))) / 2 @staticmethod + @distributionFunction def cdfinv(mean, stddev, x): import scipy # slow import not often needed @@ -1271,7 +1286,7 @@ def supportInterval(self): def __repr__(self): weights = self.weights - if all(weight == weights[0] for weight in weights): + if not weights or all(weight == weights[0] for weight in weights): return f"DiscreteRange({self.low!r}, {self.high!r})" else: return f"DiscreteRange({self.low!r}, {self.high!r}, {self.weights})" diff --git a/src/scenic/core/dynamics/behaviors.py b/src/scenic/core/dynamics/behaviors.py index 7c12ef7c2..1bab40903 100644 --- a/src/scenic/core/dynamics/behaviors.py +++ b/src/scenic/core/dynamics/behaviors.py @@ -109,7 +109,7 @@ def alarmHandler(signum, frame): try: actions = self._runningIterator.send(None) except StopIteration: - actions = () # behavior ended early + return None return actions def _stop(self, reason=None): @@ -124,18 +124,39 @@ def _isFinished(self): def _invokeInner(self, agent, subs): import scenic.syntax.veneer as veneer - assert len(subs) == 1 - sub = subs[0] - if not isinstance(sub, Behavior): - raise TypeError(f"expected a behavior, got {sub}") - sub._start(agent) - with veneer.executeInBehavior(sub): + # Validate all inner behaviors + for sub in subs: + if not isinstance(sub, Behavior): + raise TypeError(f"expected a behavior, got {sub}") + sub._start(agent) + + # Create a generator for each inner behavior that yields the appropriate actions, cleaning up when done. + def make_inner_generator(sub): try: - yield from sub._runningIterator + while sub._isRunning: + actions = sub._step() + if actions is None: + return + yield actions finally: if sub._isRunning: sub._stop() + inner_generators = [make_inner_generator(sub) for sub in subs] + + # Yield from a generator that zips all the inner generators together without padding, until all inner generators have terminated. + while True: + try: + raw_actions_list = next(itertools.zip_longest(*inner_generators)) + yield tuple( + filter( + lambda x: x is not None, + itertools.chain.from_iterable(raw_actions_list), + ) + ) + except StopIteration: + return + def __repr__(self): items = itertools.chain( (repr(arg) for arg in self._args), diff --git a/src/scenic/core/dynamics/scenarios.py b/src/scenic/core/dynamics/scenarios.py index 8aa1021b5..8d9237534 100644 --- a/src/scenic/core/dynamics/scenarios.py +++ b/src/scenic/core/dynamics/scenarios.py @@ -88,6 +88,7 @@ def __init__(self, *args, **kwargs): self._timeLimitInSteps = None # computed at simulation time self._elapsedTime = 0 + self._recordedTime = None self._eventuallySatisfied = None self._overrides = {} @@ -215,10 +216,13 @@ def _start(self): # Prepare recorders simName = veneer.currentSimulation.name + currentTime = veneer.currentSimulation.currentTime globalParams = types.MappingProxyType(veneer._globalParameters) for req in self._recordedExprs: if (recConfig := req.recConfig) and (recorder := recConfig.recorder): - recorder.beginRecording(recConfig, simName, timestep, globalParams) + recorder.beginRecording( + recConfig, simName, timestep, globalParams, currentTime + ) def _step(self): """Execute the (already-started) scenario for one time step. @@ -294,6 +298,15 @@ def _stop(self, reason, quiet=False): assert self._isRunning + if not quiet: + # Record finally-recorded values. + sim = veneer.currentSimulation + for rec in self._recordedFinalExprs: + sim._record(rec.name, rec.evaluate()) + + # Record ordinary `record` statements too if they haven't been already. + self._recordTimeSeries() + # Stop monitors and subscenarios. for monitor in self._monitors: if monitor._isRunning: @@ -356,28 +369,37 @@ def _invokeInner(self, agent, subs): # Check if any sub-scenarios stopped during action execution self._subScenarios = [sub for sub in self._subScenarios if sub._isRunning] - def _evaluateRecordedExprs(self, ty, step): - if ty is RequirementType.record: - place = "_recordedExprs" - elif ty is RequirementType.recordInitial: - place = "_recordedInitialExprs" - elif ty is RequirementType.recordFinal: - place = "_recordedFinalExprs" - else: - assert False, "invalid record type requested" - return self._evaluateRecordedExprsAt(place, step) + def _updateRecords(self): + from scenic.syntax.veneer import currentSimulation + + # _step() was called earlier this time step, so at time step 0 we will + # already have _elapsedTime == 1 + assert self._elapsedTime >= 1 + if self._elapsedTime == 1: + for rec in self._recordedInitialExprs: + currentSimulation._record(rec.name, rec.evaluate()) - def _evaluateRecordedExprsAt(self, place, step): - values = {} - for rec in getattr(self, place): + self._recordTimeSeries() + + for sub in self._subScenarios: + sub._updateRecords() + + def _recordTimeSeries(self): + from scenic.syntax.veneer import currentSimulation + + currentTime = currentSimulation.currentTime + if self._recordedTime == currentTime: + # This time step was already recorded (e.g. the scenario was terminated + # by a behavior after the current state was recorded). + return + + for rec in self._recordedExprs: value = rec.evaluate() - values[rec.name] = value + currentSimulation._recordTimeSeries(rec.name, value) if (recConfig := rec.recConfig) and (recorder := recConfig.recorder): - recorder._record(value, step) - for sub in self._subScenarios: - subvals = sub._evaluateRecordedExprsAt(place, step) - values.update(subvals) - return values + recorder._record(value, currentTime) + + self._recordedTime = currentTime def _runMonitors(self): terminationReason = None @@ -423,6 +445,14 @@ def _inherit(self, other): self._externalParameters.extend(other._externalParameters) self._requirements.extend(other._requirements) self._behaviors.extend(other._behaviors) + self._monitors.extend(other._monitors) + self._monitorRequirements.extend(other._monitorRequirements) + self._temporalRequirements.extend(other._temporalRequirements) + self._terminationConditions.extend(other._terminationConditions) + self._terminateSimulationConditions.extend(other._terminateSimulationConditions) + self._recordedExprs.extend(other._recordedExprs) + self._recordedInitialExprs.extend(other._recordedInitialExprs) + self._recordedFinalExprs.extend(other._recordedFinalExprs) def _registerInstance(self, inst): self._instances.append(inst) @@ -489,6 +519,8 @@ def _registerCompiledRequirement(self, req): place = self._terminationConditions elif req.ty is RequirementType.terminateSimulationWhen: place = self._terminateSimulationConditions + elif req.ty is RequirementType.terminateAfter: + place = self._terminationConditions elif req.ty is RequirementType.record: place = self._recordedExprs elif req.ty is RequirementType.recordInitial: @@ -558,6 +590,21 @@ def __str__(self): args = argsToString(self._args, self._kwargs) return f"{self.__class__.__name__}({args})" + def show2D(self, zoom=None, block=True): + """Render a 2D schematic of the scene for debugging.""" + import matplotlib.pyplot as plt + + plt.gca().set_aspect("equal") + # display map + self._workspace.show2D(plt) + # draw objects + for obj in self._objects: + obj.show2D(self._workspace, plt, highlight=(obj is self._ego)) + # zoom in if requested + if zoom: + self._workspace.zoomAround(plt, self._objects, expansion=zoom) + plt.show(block=block) + class LocalsSnapshot(Samplable): def __init__(self, locs): diff --git a/src/scenic/core/external_params/__init__.py b/src/scenic/core/external_params/__init__.py new file mode 100644 index 000000000..0118345f3 --- /dev/null +++ b/src/scenic/core/external_params/__init__.py @@ -0,0 +1,3 @@ +"""Scenic code related to external samplers.""" + +from scenic.core.external_params.external_params import * diff --git a/src/scenic/core/external_params/external_params.py b/src/scenic/core/external_params/external_params.py new file mode 100644 index 000000000..9ee3166d4 --- /dev/null +++ b/src/scenic/core/external_params/external_params.py @@ -0,0 +1,285 @@ +"""Support for values which are sampled outside of Scenic. + +External Samplers in General +============================ + +External samplers provide a mechanism to use different types of sampling +techniques, like optimization or quasi-random sampling, from within a Scenic +program. Ordinary random values in Scenic are instances of `Distribution`; +this module defines a special subclass, `ExternalParameter`, representing a +value which is sampled externally. Scenic programs with external parameters +are handled as follows: + + 1. During compilation, all instances of `ExternalParameter` are gathered + together and given to the `ExternalSampler.forParameters` function; + this function creates an appropriate `ExternalSampler`, + whose configuration can be controlled using :term:`global parameters` + (see the function documentation for details). + + 2. When sampling a scene, before sampling any other distributions the + :obj:`~ExternalSampler.sample` method of the `ExternalSampler` is + called to sample all the external parameters. For active samplers, this + method passes along the ``feedback`` value given to `Scenario.generate`, + if any. + + 3. Once the external parameters have values, the program is equivalent to + one without external parameters, and sampling proceeds as usual. As for + every instance of `Distribution`, the external parameters will have + their :obj:`~Samplable.sampleGiven` method called once all their + dependencies have been sampled; by default this method just returns the + value sampled for this parameter in step (2). + +.. note:: + + Note that while external parameters, like all instances of `Distribution`, + are allowed to have dependencies, they are an exception to the usual rule + that dependencies are always sampled before dependents, because the + `ExternalSampler.sample` method is called before any other sampling. + However, as explained above, the :obj:`~Samplable.sampleGiven` method is + called in the proper order and external samplers which need to do sampling + based on the values of other distributions can be invoked from it. The + two-step mechanism with `ExternalSampler.sample` is provided for samplers + which sample the whole space of external parameters at once (e.g. the + VerifAI samplers). + +Samplers from VerifAI +===================== + +The external sampling mechanism is designed to be extensible. The only built-in +`ExternalSampler` is the `VerifaiSampler`, which provides access to the +samplers in the `VerifAI`_ toolkit (which in turn can use Scenic as a modeling +language). + +The `VerifaiSampler` supports several types of external parameters corresponding +to the primitive distributions: `VerifaiRange` and `VerifaiDiscreteRange` for +continuous and discrete intervals, and `VerifaiOptions` for discrete sets. +For example, suppose we write:: + + ego = new Object at (VerifaiRange(5, 15), 0) + +This is equivalent to the ordinary Scenic line :scenic:`ego = new Object at (Range(5, 15), 0)`, +except that the X coordinate of the ego is sampled by VerifAI within the range +(5, 15) instead of being uniformly distributed over it. By default the +`VerifaiSampler` uses VerifAI's `Halton`_ sampler, so the range will still be +covered uniformly but more systematically. If we want to use a different sampler, +we can set the ``verifaiSamplerType`` global parameter:: + + param verifaiSamplerType = 'ce' + ego = new Object at (VerifaiRange(5, 15), 0) + +Now the X coordinate will be sampled using VerifAI's `cross-entropy`_ sampler. +If we pass a feedback value to `Scenario.generate` which scores the previous +scene, then the coordinate will not be sampled uniformly but rather converge to +a distribution concentrated on values minimizing the score. Active samplers like +cross-entropy can be used for falsification in this way, driving a system toward +parts of the parameter space where a specification is violated. + +The cross-entropy sampler in VerifAI can be started from a non-uniform prior. +Scenic provides a convenient way to define this prior using the ordinary syntax +for distributions:: + + param verifaiSamplerType = 'ce' + ego = new Object at (VerifaiParameter.withPrior(Normal(10, 3)), 0) + +Now cross-entropy sampling will start from a normal distribution with mean 10 +and standard deviation 3. Priors are restricted to primitive distributions and +in general may be approximated so that VerifAI can handle them -- see +`VerifaiParameter.withPrior` for details. + +To set a time bound when using VerifAI's dynamic sampling, set the ``timeBound`` +global parameter to value representing the upper bound on the number of timesteps +the sampler should account for. For example:: + + param timeBound = 250 + +This value can also be set directly in VerifAI via the ``maxSteps`` parameter to the +``ScenicSampler``. + +For more information on how to customize the sampler, see `VerifaiSampler`. + +.. _VerifAI: https://github.com/BerkeleyLearnVerify/VerifAI + +.. _Halton: https://en.wikipedia.org/wiki/Halton_sequence + +.. _cross-entropy: https://en.wikipedia.org/wiki/Cross-entropy_method + +""" + +from abc import ABC, abstractmethod +from importlib import metadata +from typing import Tuple +import warnings + +from dotmap import DotMap +import numpy + +from scenic.core.distributions import Distribution, Samplable +from scenic.core.errors import InvalidScenarioError + + +class ExternalSampler: + """Abstract class for objects called to sample values for each external parameter. + + The initializer for this class takes the same arguments as the factory function + `forParameters` below. + + Attributes: + rejectionFeedback: Value passed to the `sample` method when the last sample was rejected. + This value can be chosen by a Scenic scenario using the global parameter + ``externalSamplerRejectionFeedback``. + """ + + def __init__(self, params, globalParams): + # feedback value passed to external sampler when the last scene was rejected + self.rejectionFeedback = globalParams.get("externalSamplerRejectionFeedback") + + @classmethod + def getExternalParamType(cls): + return ExternalParameter + + @classmethod + def getExternalParameterConverter(cls): + return ExternalParameterConverter(cls, cls.getExternalParamType()) + + @staticmethod + def forParameters(params, globalParams): + """Create an `ExternalSampler` given the sets of external and global parameters. + + The scenario may explicitly select an external sampler by assigning the + :term:`global parameter` ``externalSampler`` to a subclass of `ExternalSampler`. + Otherwise, a `VerifaiSampler` is used by default. + + Args: + params (tuple): Tuple listing each `ExternalParameter`. + globalParams (dict): Dictionary of global parameters for the `Scenario`, made + available here to support sampler customization through setting parameters. + Note that the values of these parameters may be instances of `Distribution`! + + Returns: + An `ExternalSampler` configured for the given parameters. + """ + if len(params) > 0: + from scenic.core.external_params.verifai import VerifaiSampler + + externalSampler = globalParams.get("externalSampler", VerifaiSampler) + if not issubclass(externalSampler, ExternalSampler): + raise InvalidScenarioError( + f"externalSampler type {externalSampler}" + " not subclass of ExternalSampler" + ) + return externalSampler(params, globalParams) + else: + return None + + def sample(self, feedback): + """Sample values for all the external parameters. + + Args: + feedback: Feedback from the last sample (for active samplers). + """ + self.cachedSample = self.nextSample(feedback) + + def nextSample(self, feedback): + """Actually do the sampling. Implemented by subclasses.""" + raise NotImplementedError + + def valueFor(self, param): + """Return the sampled value for a parameter. Implemented by subclasses.""" + raise NotImplementedError + + +class ExternalParameter(Distribution): + """A value determined by external code rather than Scenic's internal sampler.""" + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self.sampler = None + self.isTimeSeries = False + import scenic.syntax.veneer as veneer # TODO improve? + + veneer.registerExternalParameter(self) + + def sampleGiven(self, value): + """Specialization of `Samplable.sampleGiven` for external parameters. + + By default, this method simply looks up the value previously sampled by + `ExternalSampler.sample`. + """ + assert self.sampler is not None + return self.sampler.valueFor(self) + + def extractOutput(self, value): + """ + Given a raw sampled value for a parameter, optionally extract the actual desired value. + + By default just passes the value through unchanged. + """ + return value + + +class ExternalParameterConverter: + def __init__(self, samplerType, paramType): + self.samplerType = samplerType + self.paramType = paramType + self.externalParams = [] + + def registerExternalParams(self, dist): + for d in dist.recursiveDependencies(): + if isinstance(d, self.paramType): + self.externalParams.append(d) + + def convert(self, samp): + if not isinstance(samp, Samplable): + return + + sampCond = samp._conditioned + + if isinstance(sampCond, self.paramType) or not isinstance(sampCond, Distribution): + for dep in sampCond._dependencies: + self.convert(dep) + else: + try: + self.convertInner(samp) + except NotImplementedError: + warnings.warn( + f"Unable to convert {type(sampCond)} to {self.paramType} based distribution." + ) + + def convertInner(self, dist): + """Condition dist to an equivalent `ExternalParameter` based sampblable. + + If such a conversion is not possible, this function should raise NotImplementedError. + """ + raise NotImplementedError + + +class TimeSeriesParameter: + def __init__(self, callback): + self._callback = callback + self._lastSimulation = None + self._lastTime = -1 + + def getSample(self): + import scenic.syntax.veneer as veneer + + assert veneer.currentSimulation is not None + + if self._lastSimulation is not veneer.currentSimulation: + self._lastSimulation = veneer.currentSimulation + self._lastTime = -1 + + if veneer.currentSimulation.currentTime <= self._lastTime: + raise RuntimeError( + "Attempted `getSample` for a TimeSeries external parameter twice in one timestep." + ) + + self._lastTime = veneer.currentSimulation.currentTime + return self._callback() + + +def TimeSeries(param): + if not isinstance(param, ExternalParameter): + raise TypeError("Cannot turn a non `ExternalParameter` into a time series") + + param.isTimeSeries = True + return param diff --git a/src/scenic/core/external_params/optuna.py b/src/scenic/core/external_params/optuna.py new file mode 100644 index 000000000..6220b4c15 --- /dev/null +++ b/src/scenic/core/external_params/optuna.py @@ -0,0 +1,227 @@ +from abc import abstractmethod +import math + +import optuna + +from scenic.core.distributions import ( + DiscreteRange, + Normal, + Options, + Range, + RejectionException, + UniformDistribution, + toDistribution, +) +from scenic.core.external_params.external_params import * +from scenic.core.lazy_eval import valueInContext +from scenic.core.regions import PointInRegionDistribution +import scenic.syntax.veneer + + +class OptunaSampler(ExternalSampler): + def __init__(self, params, globalParams): + super().__init__(params, globalParams) + + optuna.logging.set_verbosity( + globalParams.get("optunaLogLevel", optuna.logging.WARNING) + ) + + self.sampler = globalParams.get("optunaSampler", None) + self.study_name = globalParams.get("optunaStudyName", None) + self.storage = globalParams.get("optunaStorage", None) + self.study = optuna.create_study( + sampler=self.sampler, + study_name=self.study_name, + pruner=optuna.pruners.NopPruner(), + storage=self.storage, + direction="minimize", + ) + self.params = tuple(params) + for index, param in enumerate(self.params): + if not isinstance(param, OptunaParameter): + raise RuntimeError( + f"OptunaSampler given parameter of wrong type: {param}" + ) + param.sampler = self + param.index = index + + @classmethod + def getExternalParamType(cls): + return OptunaParameter + + @classmethod + def getExternalParameterConverter(cls): + return OptunaParameterConverter(cls, cls.getExternalParamType()) + + @property + def trial(self): + return self.cachedSample + + def nextSample(self, feedback): + if feedback is not None: + if self.study_name is None: + warnings.warn( + "Feedback passed without setting Optuna study name. Feedback will not have any effect." + ) + assert self.trial is not None + self.study.tell(self.trial, feedback) + + return self.study.ask() + + def valueFor(self, param, value): + if param.isTimeSeries: + raise ValueError( + "OptunaSampler does currently support timeSeries parameters." + ) + + return param.suggestValue(self, value) + + +class OptunaParameterConverter(ExternalParameterConverter): + def convertInner(self, dist): + distCond = dist._conditioned + if isinstance(distCond, Range): + self.convert(distCond.low) + self.convert(distCond.high) + newDist = OptunaRange(distCond.low, distCond.high) + self.registerExternalParams(newDist) + dist.conditionTo(newDist) + elif isinstance(distCond, DiscreteRange): + self.convert(distCond.low) + self.convert(distCond.high) + newDist = OptunaDiscreteRange(distCond.low, distCond.high) + self.registerExternalParams(newDist) + dist.conditionTo(newDist) + elif isinstance(distCond, Normal): + self.convert(distCond.stddev) + self.convert(distCond.mean) + newDist = Normal.cdfinv(distCond.mean, distCond.stddev, OptunaRange(-1, 1)) + self.registerExternalParams(newDist) + dist.conditionTo(newDist) + elif isinstance(distCond, Options) and not isinstance(distCond, OptunaOptions): + for o in distCond.options: + self.convert(o) + newDist = OptunaOptions(distCond.options) + self.registerExternalParams(newDist) + dist.conditionTo(newDist) + elif isinstance(distCond, UniformDistribution): + for o in distCond.options: + self.convert(o) + newSelector = _OptunaCategoricalHelper( + distCond.selector.high, emptyMessage="Empty Optuna UniformDistribution." + ) + self.registerExternalParams(newSelector) + distCond.selector.conditionTo(newSelector) + elif ( + isinstance(distCond, PointInRegionDistribution) + and not distCond._deterministic + ): + self.convert(distCond.region) + + # def makeSampleVals(dims): + # if isinstance(dims, tuple): + # return toDistribution(tuple(makeSampleVals(dim) for dim in dims)) + # elif isinstance(dims, int): + # return toDistribution(tuple(OptunaRange(0,1) for _ in range(dims))) + # else: + # assert False, dims + # sampleVals = makeSampleVals(dist.region._sampleVals) + + sampleVals = toDistribution(tuple(OptunaRange(0, 1) for _ in range(3))) + self.registerExternalParams(sampleVals) + newDist = PointInRegionDistribution( + region=distCond.region, tag=distCond.tag, sampleVals=sampleVals + ) + dist.conditionTo(newDist) + elif distCond._deterministic: + for dep in distCond._dependencies: + self.convert(dep) + else: + raise NotImplementedError + + +class OptunaParameter(ExternalParameter): + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self.index = None + + def sampleGiven(self, value): + """Specialization of `Samplable.sampleGiven` for external parameters. + + By default, this method simply looks up the value previously sampled by + `ExternalSampler.sample`. + """ + assert self.sampler is not None + return self.sampler.valueFor(self, value) + + @abstractmethod + def suggestValue(self, trial): + pass + + def optunaName(self, extra=None): + assert self.index is not None + return f"{type(self).__name__}_{self.index}_{extra}" + + +class OptunaRange(OptunaParameter): + """A :obj:`~scenic.core.distributions.Range` (real interval) sampled by VerifAI.""" + + _defaultValueType = float + + def __init__(self, low, high): + super().__init__(low, high) + self.low = low + self.high = high + + def suggestValue(self, sampler, value): + return sampler.trial.suggest_float( + self.optunaName(), value[self.low], value[self.high] + ) + + +class OptunaDiscreteRange(OptunaParameter): + """A :obj:`~scenic.core.distributions.DiscreteRange` (integer interval) sampled by Optuna.""" + + _defaultValueType = int + + def __init__(self, low, high, emptyMessage=None): + super().__init__(low, high) + self.low = low + self.high = high + self.emptyMessage = emptyMessage + + def suggestValue(self, sampler, value): + if value[self.low] > value[self.low]: + raise RejectionException(self.emptyMessage if self.emptyMessage else "") + return sampler.trial.suggest_int( + self.optunaName((value[self.low], value[self.low])), + value[self.low], + value[self.high], + ) + + +class _OptunaCategoricalHelper(OptunaParameter): + _defaultValueType = int + + def __init__(self, numOptions, emptyMessage): + super().__init__(numOptions) + self.numOptions = numOptions + self.emptyMessage = emptyMessage + + def suggestValue(self, sampler, value): + optionsNums = list(range(value[self.numOptions] + 1)) + if len(optionsNums) == 0: + raise RejectionException(self.emptyMessage) + return sampler.trial.suggest_categorical( + self.optunaName(value[self.numOptions]), optionsNums + ) + + +class OptunaOptions(Options): + """An :obj:`~scenic.core.distributions.Options` (discrete set) sampled by Optuna.""" + + @staticmethod + def makeSelector(n, weights): + if weights: + warnings.warn("Ignoring weights passed to OptunaOptions.") + return _OptunaCategoricalHelper(n, emptyMessage="Empty Optuna Options.") diff --git a/src/scenic/core/external_params.py b/src/scenic/core/external_params/verifai.py similarity index 50% rename from src/scenic/core/external_params.py rename to src/scenic/core/external_params/verifai.py index 211cf22a9..3b25249ef 100644 --- a/src/scenic/core/external_params.py +++ b/src/scenic/core/external_params/verifai.py @@ -1,180 +1,5 @@ -"""Support for values which are sampled outside of Scenic. - -External Samplers in General -============================ - -External samplers provide a mechanism to use different types of sampling -techniques, like optimization or quasi-random sampling, from within a Scenic -program. Ordinary random values in Scenic are instances of `Distribution`; -this module defines a special subclass, `ExternalParameter`, representing a -value which is sampled externally. Scenic programs with external parameters -are handled as follows: - - 1. During compilation, all instances of `ExternalParameter` are gathered - together and given to the `ExternalSampler.forParameters` function; - this function creates an appropriate `ExternalSampler`, - whose configuration can be controlled using :term:`global parameters` - (see the function documentation for details). - - 2. When sampling a scene, before sampling any other distributions the - :obj:`~ExternalSampler.sample` method of the `ExternalSampler` is - called to sample all the external parameters. For active samplers, this - method passes along the ``feedback`` value given to `Scenario.generate`, - if any. - - 3. Once the external parameters have values, the program is equivalent to - one without external parameters, and sampling proceeds as usual. As for - every instance of `Distribution`, the external parameters will have - their :obj:`~Samplable.sampleGiven` method called once all their - dependencies have been sampled; by default this method just returns the - value sampled for this parameter in step (2). - -.. note:: - - Note that while external parameters, like all instances of `Distribution`, - are allowed to have dependencies, they are an exception to the usual rule - that dependencies are always sampled before dependents, because the - `ExternalSampler.sample` method is called before any other sampling. - However, as explained above, the :obj:`~Samplable.sampleGiven` method is - called in the proper order and external samplers which need to do sampling - based on the values of other distributions can be invoked from it. The - two-step mechanism with `ExternalSampler.sample` is provided for samplers - which sample the whole space of external parameters at once (e.g. the - VerifAI samplers). - -Samplers from VerifAI -===================== - -The external sampling mechanism is designed to be extensible. The only built-in -`ExternalSampler` is the `VerifaiSampler`, which provides access to the -samplers in the `VerifAI`_ toolkit (which in turn can use Scenic as a modeling -language). - -The `VerifaiSampler` supports several types of external parameters corresponding -to the primitive distributions: `VerifaiRange` and `VerifaiDiscreteRange` for -continuous and discrete intervals, and `VerifaiOptions` for discrete sets. -For example, suppose we write:: - - ego = new Object at (VerifaiRange(5, 15), 0) - -This is equivalent to the ordinary Scenic line :scenic:`ego = new Object at (Range(5, 15), 0)`, -except that the X coordinate of the ego is sampled by VerifAI within the range -(5, 15) instead of being uniformly distributed over it. By default the -`VerifaiSampler` uses VerifAI's `Halton`_ sampler, so the range will still be -covered uniformly but more systematically. If we want to use a different sampler, -we can set the ``verifaiSamplerType`` global parameter:: - - param verifaiSamplerType = 'ce' - ego = new Object at (VerifaiRange(5, 15), 0) - -Now the X coordinate will be sampled using VerifAI's `cross-entropy`_ sampler. -If we pass a feedback value to `Scenario.generate` which scores the previous -scene, then the coordinate will not be sampled uniformly but rather converge to -a distribution concentrated on values minimizing the score. Active samplers like -cross-entropy can be used for falsification in this way, driving a system toward -parts of the parameter space where a specification is violated. - -The cross-entropy sampler in VerifAI can be started from a non-uniform prior. -Scenic provides a convenient way to define this prior using the ordinary syntax -for distributions:: - - param verifaiSamplerType = 'ce' - ego = new Object at (VerifaiParameter.withPrior(Normal(10, 3)), 0) - -Now cross-entropy sampling will start from a normal distribution with mean 10 -and standard deviation 3. Priors are restricted to primitive distributions and -in general may be approximated so that VerifAI can handle them -- see -`VerifaiParameter.withPrior` for details. - -To set a time bound when using VerifAI's dynamic sampling, set the ``timeBound`` -global parameter to value representing the upper bound on the number of timesteps -the sampler should account for. For example:: - - param timeBound = 250 - -This value can also be set directly in VerifAI via the ``maxSteps`` parameter to the -``ScenicSampler``. - -For more information on how to customize the sampler, see `VerifaiSampler`. - -.. _VerifAI: https://github.com/BerkeleyLearnVerify/VerifAI - -.. _Halton: https://en.wikipedia.org/wiki/Halton_sequence - -.. _cross-entropy: https://en.wikipedia.org/wiki/Cross-entropy_method - -""" - -from abc import ABC, abstractmethod -from importlib import metadata -import warnings - -from dotmap import DotMap -import numpy - -from scenic.core.distributions import Distribution, Options -from scenic.core.errors import InvalidScenarioError - - -class ExternalSampler: - """Abstract class for objects called to sample values for each external parameter. - - The initializer for this class takes the same arguments as the factory function - `forParameters` below. - - Attributes: - rejectionFeedback: Value passed to the `sample` method when the last sample was rejected. - This value can be chosen by a Scenic scenario using the global parameter - ``externalSamplerRejectionFeedback``. - """ - - def __init__(self, params, globalParams): - # feedback value passed to external sampler when the last scene was rejected - self.rejectionFeedback = globalParams.get("externalSamplerRejectionFeedback") - - @staticmethod - def forParameters(params, globalParams): - """Create an `ExternalSampler` given the sets of external and global parameters. - - The scenario may explicitly select an external sampler by assigning the - :term:`global parameter` ``externalSampler`` to a subclass of `ExternalSampler`. - Otherwise, a `VerifaiSampler` is used by default. - - Args: - params (tuple): Tuple listing each `ExternalParameter`. - globalParams (dict): Dictionary of global parameters for the `Scenario`, made - available here to support sampler customization through setting parameters. - Note that the values of these parameters may be instances of `Distribution`! - - Returns: - An `ExternalSampler` configured for the given parameters. - """ - if len(params) > 0: - externalSampler = globalParams.get("externalSampler", VerifaiSampler) - if not issubclass(externalSampler, ExternalSampler): - raise InvalidScenarioError( - f"externalSampler type {externalSampler}" - " not subclass of ExternalSampler" - ) - return externalSampler(params, globalParams) - else: - return None - - def sample(self, feedback): - """Sample values for all the external parameters. - - Args: - feedback: Feedback from the last sample (for active samplers). - """ - self.cachedSample = self.nextSample(feedback) - - def nextSample(self, feedback): - """Actually do the sampling. Implemented by subclasses.""" - raise NotImplementedError - - def valueFor(self, param): - """Return the sampled value for a parameter. Implemented by subclasses.""" - raise NotImplementedError +from scenic.core.distributions import Options +from scenic.core.external_params.external_params import * class VerifaiSampler(ExternalSampler): @@ -349,67 +174,6 @@ def nameForParam(i): return f"param{i}" -class ExternalParameter(Distribution): - """A value determined by external code rather than Scenic's internal sampler.""" - - def __init__(self): - super().__init__() - self.sampler = None - self.isTimeSeries = False - import scenic.syntax.veneer as veneer # TODO improve? - - veneer.registerExternalParameter(self) - - def sampleGiven(self, value): - """Specialization of `Samplable.sampleGiven` for external parameters. - - By default, this method simply looks up the value previously sampled by - `ExternalSampler.sample`. - """ - assert self.sampler is not None - return self.sampler.valueFor(self) - - def extractOutput(self, value): - """ - Given a raw sampled value for a parameter, optionally extract the actual desired value. - - By default just passes the value through unchanged. - """ - return value - - -class TimeSeriesParameter: - def __init__(self, callback): - self._callback = callback - self._lastSimulation = None - self._lastTime = -1 - - def getSample(self): - import scenic.syntax.veneer as veneer - - assert veneer.currentSimulation is not None - - if self._lastSimulation is not veneer.currentSimulation: - self._lastSimulation = veneer.currentSimulation - self._lastTime = -1 - - if veneer.currentSimulation.currentTime <= self._lastTime: - raise RuntimeError( - "Attempted `getSample` for a TimeSeries external parameter twice in one timestep." - ) - - self._lastTime = veneer.currentSimulation.currentTime - return self._callback() - - -def TimeSeries(param): - if not isinstance(param, ExternalParameter): - raise TypeError("Cannot turn a non `ExternalParameter` into a time series") - - param.isTimeSeries = True - return param - - class VerifaiParameter(ExternalParameter): """An external parameter sampled using one of VerifAI's samplers.""" diff --git a/src/scenic/core/geometry.py b/src/scenic/core/geometry.py index b60a68b2d..b28bcf24a 100644 --- a/src/scenic/core/geometry.py +++ b/src/scenic/core/geometry.py @@ -2,6 +2,7 @@ import itertools import math +from typing import Iterable import warnings import numpy as np @@ -14,6 +15,7 @@ needsSampling, ) from scenic.core.lazy_eval import isLazy +from scenic.core.type_support import toVector from scenic.core.utils import cached_property @@ -27,6 +29,11 @@ def cos(x) -> float: return math.cos(x) +@distributionFunction +def tan(x) -> float: + return math.tan(x) + + @monotonicDistributionFunction def hypot(*args) -> float: return math.hypot(*args) @@ -110,7 +117,9 @@ def distanceToLine(point, a, b): # Fastest known way to make a Shapely Point from a list/tuple/Vector -makeShapelyPoint = shapely.points +makeShapelyPoint = lambda pt: ( + shapely.points(pt) if isinstance(pt, Iterable) else shapely.points(toVector(pt)) +) def polygonUnion(polys, buf=0, tolerance=0, holeTolerance=0.002): diff --git a/src/scenic/core/object_types.py b/src/scenic/core/object_types.py index 11ff01004..9b80debf0 100644 --- a/src/scenic/core/object_types.py +++ b/src/scenic/core/object_types.py @@ -988,6 +988,10 @@ def distancePast(self, vec): diff = self.position - vec return diff.rotatedBy(-self.heading).y + def apparentHeadingTo(self, vec): + """The apparent heading to a given point, from the perspective of this `OrientedPoint`.""" + return normalizeAngle(self.position.angleTo(vec) - self.heading) + def toHeading(self) -> float: return self.heading @@ -1168,6 +1172,20 @@ def distanceTo(self, point): """The minimal distance from the space this object occupies to a given point""" return self.occupiedSpace.distanceTo(point) + @cached_method + def minimumDistanceTo(self, other): + """The minimal distance between this object and another.""" + if not isinstance(other, Object): + raise RuntimeError( + f"Cannot compute minimum distance between Object and {type(other)} " + ) + + # 2D fast path + if self._isPlanarBox and other._isPlanarBox and self.z == other.z: + return self._boundingPolygon.distance(other._boundingPolygon) + + return self.occupiedSpace.minimumDistanceTo(other.occupiedSpace) + @cached_method def intersects(self, other): """Whether or not this object intersects another object or region""" diff --git a/src/scenic/core/regions.py b/src/scenic/core/regions.py index 13afc1348..1151658d8 100644 --- a/src/scenic/core/regions.py +++ b/src/scenic/core/regions.py @@ -8,9 +8,11 @@ """ from abc import ABC, abstractmethod +import bisect import itertools import math import random +from typing import Iterable import warnings import fcl @@ -57,7 +59,7 @@ triangulatePolygon, ) from scenic.core.lazy_eval import isLazy, valueInContext -from scenic.core.type_support import toOrientation, toScalar, toVector +from scenic.core.type_support import canCoerce, toOrientation, toScalar, toVector from scenic.core.utils import ( cached, cached_method, @@ -78,6 +80,11 @@ ################################################################################################### +# TODO: Move this somewhere else? +def randomIndexFromVal(val, weights): + return bisect.bisect(weights, val * weights[-1]) + + class Region(Samplable, ABC): """An abstract base class for Scenic Regions""" @@ -92,6 +99,14 @@ def uniformPointInner(self): """Do the actual random sampling. Implemented by subclasses.""" pass + @abstractmethod + def parameterizedUniformPointInner(self, vals): + """Sample from this region deterministically, given vals. Implemented by subclasses. + + vals contains at least 3 distributions in [0,1]. + """ + pass + @abstractmethod def containsPoint(self, point) -> bool: """Check if the `Region` contains a point. Implemented by subclasses.""" @@ -117,6 +132,11 @@ def projectVector(self, point, onDirection): """Returns point projected onto this region along onDirection.""" pass + @abstractmethod + def closestPointTo(self, target): + """Returns the closest point to target (also a point) that is contained in the region""" + pass + @property @abstractmethod def AABB(self): @@ -135,6 +155,11 @@ def dimensionality(self): def size(self): return None + @property + @abstractmethod + def _sampleVals(self): + pass + def intersects(self, other, triedReversed=False) -> bool: """intersects(other) @@ -262,17 +287,27 @@ def __repr__(self): s += f" {self.name}" return s + f" at {hex(id(self))}>" + @cached_property + def midpoint(self): + return Vector(*self.AABB[0]) + Vector(*self.AABB[1]) / 2 + class PointInRegionDistribution(VectorDistribution): """Uniform distribution over points in a Region""" - def __init__(self, region, tag=None): - super().__init__(region) + def __init__(self, region, tag=None, sampleVals=None): + super().__init__(region, sampleVals) self.region = region self.tag = tag + self.sampleVals = sampleVals def sampleGiven(self, value): - return value[self.region].uniformPointInner() + if self.sampleVals is None: + return value[self.region].uniformPointInner() + else: + return value[self.region].parameterizedUniformPointInner( + value[self.sampleVals] + ) @property def heading(self): @@ -294,6 +329,10 @@ def z(self) -> float: def __repr__(self): return f"PointIn({self.region!r})" + @property + def _deterministic(self): + return self.sampleVals is not None + ################################################################################################### # Utility Regions and Functions @@ -315,6 +354,9 @@ def union(self, other, triedReversed=False): def uniformPointInner(self): raise RuntimeError(f"Attempted to sample from everywhere (AllRegion)") + def parameterizedUniformPointInner(self, vals): + raise RuntimeError(f"Attempted to sample from everywhere (AllRegion)") + def containsPoint(self, point): return True @@ -330,10 +372,17 @@ def distanceTo(self, point): def projectVector(self, point, onDirection): return point + def closestPointTo(self, target): + return toVector(target) + @property def AABB(self): raise TypeError("AllRegion does not have a well defined AABB") + @property + def _sampleVals(self): + return (0,) + @property def dimensionality(self): return float("inf") @@ -367,6 +416,9 @@ def union(self, other, triedReversed=False): def uniformPointInner(self): raise RejectionException(f"sampling empty Region") + def parameterizedUniformPointInner(self, vals): + raise RejectionException(f"sampling empty Region") + def containsPoint(self, point): return False @@ -382,10 +434,17 @@ def distanceTo(self, point): def projectVector(self, point, onDirection): raise RejectionException("Projecting vector onto empty Region") + def closestPointTo(self, target): + raise RejectionException("Finding closest point in empty Region") + @property def AABB(self): raise TypeError("EmptyRegion does not have a well defined AABB") + @property + def _sampleVals(self): + return (0,) + @property def dimensionality(self): return 0 @@ -469,10 +528,18 @@ def projectVector(self, point, onDirection): f'{type(self).__name__} does not yet support projection using "on"' ) + def closestPointTo(self, target): + raise NotImplementedError + @property def AABB(self): raise NotImplementedError + @property + def _sampleVals(self): + raise NotImplementedError + return tuple(r._sampleVals for r in self.regions) + @cached_property def footprint(self): return convertToFootprint(self) @@ -483,8 +550,12 @@ def uniformPointInner(self): sampler = self.genericSampler return self.orient(sampler(self)) + def parameterizedUniformPointInner(self, vals): + assert len(vals) == len(self.regions) + return self.orient(self.genericSampler(self, vals)) + @staticmethod - def genericSampler(intersection): + def genericSampler(intersection, vals=None): regs = intersection.regions # Filter out all regions with known dimensionality greater than the minimum known_dim_regions = [ @@ -500,7 +571,10 @@ def genericSampler(intersection): for reg in sampling_regions: try: - point = reg.uniformPointInner() + if vals is None: + point = reg.uniformPointInner() + else: + point = reg.parameterizedUniformPointInner(vals[regs.index(reg)]) except UndefinedSamplingException: num_regs_undefined += 1 continue @@ -582,10 +656,21 @@ def projectVector(self, point, onDirection): f'{type(self).__name__} does not yet support projection using "on"' ) + def closestPointTo(self, target): + target = toVector(target) + candidate_points = [region.closestPointTo(target) for region in self.regions] + candidate_points.sort(key=lambda pt: pt.distanceTo(target)) + return candidate_points[0] + @property def AABB(self): raise NotImplementedError + @property + def _sampleVals(self): + raise NotImplementedError + return (1,) + tuple(r._sampleVals for r in self.regions) + @cached_property def footprint(self): return convertToFootprint(self) @@ -596,8 +681,12 @@ def uniformPointInner(self): sampler = self.genericSampler return self.orient(sampler(self)) + def parameterizedUniformPointInner(self, vals): + assert len(vals) == 1 + len(self.regions) + return self.orient(self.genericSampler(self, vals)) + @staticmethod - def genericSampler(union): + def genericSampler(union, vals=None): regs = union.regions # Check that all regions have well defined dimensionality @@ -613,13 +702,21 @@ def genericSampler(union): # Check that all large regions have well defined size if any(reg.size is None or reg.size == float("inf") for reg in large_regs): raise UndefinedSamplingException( - f"cannot sample union of Regions {regs} with " "ill-defined size" + f"cannot sample union of Regions {regs} with ill-defined size" ) # Pick a sample, weighted by region size reg_sizes = tuple(reg.size for reg in large_regs) - target_reg = random.choices(large_regs, weights=reg_sizes)[0] - point = target_reg.uniformPointInner() + + if vals is None: + target_reg = random.choices(large_regs, weights=reg_sizes)[0] + point = target_reg.uniformPointInner() + else: + target_values = vals[1 + regs.index(target_reg)] + cum_sizes = list(itertools.accumulate(reg_sizes)) + target_reg = large_regs[randomIndexFromVal(vals[0], cum_sizes)] + + point = target_reg.parameterizedUniformPointInner(target_values) # Potentially reject based on containment of the sample containment_count = sum(int(reg._trueContainsPoint(point)) for reg in regs) @@ -683,10 +780,18 @@ def projectVector(self, point, onDirection): f'{type(self).__name__} does not yet support projection using "on"' ) + def closestPointTo(self, target): + raise NotImplementedError + @property def AABB(self): raise NotImplementedError + @property + def _sampleVals(self): + raise NotImplementedError + return self.regionA._sampleVals + @cached_property def footprint(self): return convertToFootprint(self) @@ -697,10 +802,16 @@ def uniformPointInner(self): sampler = self.genericSampler return self.orient(sampler(self)) + def parameterizedUniformPointInner(self): + return self.orient(self.genericSampler(self)) + @staticmethod - def genericSampler(difference): + def genericSampler(difference, vals=None): regionA, regionB = difference.regionA, difference.regionB - point = regionA.uniformPointInner() + if vals is None: + point = regionA.uniformPointInner() + else: + point = regionA.parameterizedUniformPointInner(vals) if regionB._trueContainsPoint(point): raise RejectionException( f"sampling difference of Regions {regionA} and {regionB}" @@ -720,12 +831,17 @@ def toPolygon(thing): poly = thing.polygons elif hasattr(thing, "lineString"): poly = thing.lineString + elif isinstance(thing, (Iterable, Vector)): + poly = makeShapelyPoint(thing) else: return None return poly +toShapely = toPolygon + + def regionFromShapelyObject(obj, orientation=None): """Build a 'Region' from Shapely geometry.""" assert obj.is_valid, obj @@ -1000,6 +1116,10 @@ def projectVector(self, point, onDirection): return Vector(*closest_point) + @distributionFunction + def closestPointTo(self, target): + return toVector(trimesh.proximity.closest_point(self.mesh, target)) + @cached_property @distributionFunction def circumcircle(self): @@ -1744,6 +1864,16 @@ def uniformPointInner(self): else: return Vector(*sample[0]) + def parameterizedUniformPointInner(self, vals): + # assert len(vals) == 3 + point = Vector( + *(numpy.asarray(vals[:3]) * self.mesh.extents + self.mesh.bounds[0]) + ) + if self.containsPoint(point): + return point + else: + raise RejectionException + @distributionFunction def distanceTo(self, point): """Get the minimum distance from this region to the specified point.""" @@ -1759,6 +1889,31 @@ def distanceTo(self, point): return abs(dist) + @distributionFunction + def closestPointTo(self, target): + target = toVector(target) + + if self.containsPoint(target): + return target + + return super().closestPointTo(target) + + @distributionFunction + def minimumDistanceTo(self, other): + """Get the minimum distance between this region and another. + + Currently only supports other as a `MeshVolumeRegion`, and is + primarily used for computing minimum distance between objects. + """ + if not isinstance(other, MeshVolumeRegion): + raise NotImplementedError( + f"Cannot compute distance between MeshVolumeRegion and {type(other)}" + ) + + selfObj = fcl.CollisionObject(*self._fclData) + otherObj = fcl.CollisionObject(*other._fclData) + return fcl.distance(selfObj, otherObj) + @cached_property @distributionFunction def inradius(self): @@ -1780,6 +1935,10 @@ def isConvex(self): def dimensionality(self): return 3 + @property + def _sampleVals(self): + return (3,) + @cached_property def size(self): return self.mesh.mass / self.mesh.density @@ -2075,6 +2234,9 @@ def containsRegionInner(self, reg, tolerance): def uniformPointInner(self): return Vector(*trimesh.sample.sample_surface(self.mesh, 1)[0][0]) + def parameterizedUniformPointInner(self, vals): + raise NotImplementedError # TODO + @distributionFunction def distanceTo(self, point): """Get the minimum distance from this object to the specified point.""" @@ -2090,6 +2252,10 @@ def distanceTo(self, point): def dimensionality(self): return 2 + @property + def _sampleVals(self): + return (3,) + @cached_property def size(self): return self.mesh.area @@ -2289,6 +2455,9 @@ def distanceTo(self, point): def projectVector(self, point, onDirection): raise NotImplementedError + def closestPointTo(self, target): + raise NotImplementedError + def uniformPointInner(self): # First generate a point uniformly in a box with dimensions # equal to scale, centered at the origin. @@ -2301,6 +2470,9 @@ def uniformPointInner(self): return Vector(*offset_pt) + def parameterizedUniformPointInner(self, vals): + raise NotImplementedError() + def dilation(self, iterations, structure=None): """Returns a dilated/eroded version of this VoxelRegion. @@ -2428,6 +2600,10 @@ def AABB(self): tuple(self.voxelGrid.bounds[1]), ) + @property + def _sampleVals(self): + return (3,) + @property def size(self): return self.voxelGrid.volume @@ -2526,6 +2702,11 @@ def uniformPointInner(self): f"Attempted to sample from a PolygonalFootprintRegion, for which uniform sampling is undefined" ) + def parameterizedUniformPointInner(self): + raise UndefinedSamplingException( + f"Attempted to sample from a PolygonalFootprintRegion, for which uniform sampling is undefined" + ) + def containsPoint(self, point): """Checks if a point is contained in the polygonal footprint. @@ -2560,10 +2741,10 @@ def containsObject(self, obj): def containsRegionInner(self, reg, tolerance): buffered_polygons = self.polygons.buffer(tolerance) - if isinstance(other, MeshRegion): + if isinstance(reg, MeshRegion): return buffered_polygons.contains(reg._boundingPolygon) - if isinstance(other, (PolygonalRegion, PolygonalFootprintRegion)): + if isinstance(reg, (PolygonalRegion, PolygonalFootprintRegion)): return buffered_polygons.contains(reg.polygons) raise NotImplementedError @@ -2579,6 +2760,12 @@ def projectVector(self, point, onDirection): f'{type(self).__name__} does not yet support projection using "on"' ) + @distributionFunction + def closestPointTo(self, target): + target = toVector(target) + pt_2d = toVector(shapely.ops.nearest_points(self.polygons, toShapely(target))[0]) + return Vector(pt_2d.x, pt_2d.y, target.z) + @property def AABB(self): raise NotImplementedError @@ -2587,6 +2774,10 @@ def AABB(self): def dimensionality(self): return 3 + @property + def _sampleVals(self): + return (0,) + @property def size(self): return float("inf") @@ -2760,6 +2951,8 @@ def __init__( self.edge_lengths.append(c1.distanceTo(c2)) + self.cum_edge_lengths = tuple(itertools.accumulate(self.edge_lengths)) + self.tolerance = tolerance self._edgeVectorArray = numpy.asarray( @@ -2818,6 +3011,9 @@ def defaultOrientation(self, point): def projectVector(self, point, onDirection): raise NotImplementedError + def closestPointTo(self, target): + raise NotImplementedError + @cached_property def AABB(self): return ( @@ -2825,14 +3021,30 @@ def AABB(self): tuple(numpy.amax(self.vertices, axis=0)), ) + @property + def _sampleVals(self): + return (1,) + def uniformPointInner(self): - # Pick an edge, weighted by length, and extract its two points - edge = random.choices(population=self.edges, weights=self.edge_lengths, k=1)[0] - v1, v2 = edge - c1, c2 = self.vert_to_vec[v1], self.vert_to_vec[v2] + return self.parameterizedUniformPointInner([random.random()]) + + def parameterizedUniformPointInner(self, vals): + # assert len(vals) == 1 + + # Pick a random length along the path, and sample the point at that distance along the path. + length_along = vals[0] * self.cum_edge_lengths[-1] + edge_i = randomIndexFromVal(vals[0], self.cum_edge_lengths) + edge_dist = ( + length_along + if edge_i == 0 + else length_along - self.cum_edge_lengths[edge_i - 1] + ) + edge_fraction = edge_dist / self.edge_lengths[edge_i] + v1, v2 = self.edges[edge_i] + c1, c2 = self.vert_to_vec[v1], self.vert_to_vec[v2] # Sample uniformly from the line segment - sampled_pt = c1 + random.uniform(0, 1) * (c2 - c1) + sampled_pt = c1 + edge_fraction * (c2 - c1) return sampled_pt @@ -2842,7 +3054,7 @@ def dimensionality(self): @cached_property def size(self): - return sum(self.edge_lengths) + return self.cum_edge_lengths[-1] ################################################################################################### @@ -2969,14 +3181,36 @@ def _samplingData(self): return trianglesAndBounds, cumulativeTriangleAreas def uniformPointInner(self): + return self.parameterizedUniformPointInner( + tuple(random.random() for _ in range(3)) + ) + # trisAndBounds, cumulativeAreas = self._samplingData + # triangle, bounds = random.choices(trisAndBounds, cum_weights=cumulativeAreas)[0] + # minx, miny, maxx, maxy = bounds + # # TODO improve? + # while True: + # x, y = random.uniform(minx, maxx), random.uniform(miny, maxy) + # if shapely.intersects_xy(triangle, x, y): + # return self.orient(Vector(x, y, self.z)) + + def parameterizedUniformPointInner(self, vals): + # assert len(vals) == 3 + trisAndBounds, cumulativeAreas = self._samplingData - triangle, bounds = random.choices(trisAndBounds, cum_weights=cumulativeAreas)[0] - minx, miny, maxx, maxy = bounds - # TODO improve? - while True: - x, y = random.uniform(minx, maxx), random.uniform(miny, maxy) - if shapely.intersects_xy(triangle, x, y): - return self.orient(Vector(x, y, self.z)) + triangle, _ = trisAndBounds[randomIndexFromVal(vals[0], cumulativeAreas)] + + # Pick a point in the parallelogram of this triangle and its reflection + p1, p2, p3 = [Vector(*p) for p in triangle.exterior.coords[:3]] + v1 = p2 - p1 + v2 = p3 - p1 + pt = p1 + vals[1] * v1 + vals[2] * v2 + + # If the point is in the triangle's reflection, reflect it back. + if not triangle.contains(toShapely(pt)): + midpoint = (p3 + p2) / 2 + pt = pt + 2 * (midpoint - pt) + + return self.orient(Vector(pt.x, pt.y, self.z)) @distributionFunction def intersects(self, other, triedReversed=False): @@ -3114,6 +3348,12 @@ def distanceTo(self, point): dist2D = shapely.distance(self.polygons, makeShapelyPoint(point)) return math.hypot(dist2D, point[2] - self.z) + @distributionFunction + def closestPointTo(self, target): + target = toVector(target) + pt_2d = toVector(shapely.ops.nearest_points(self.polygons, toShapely(target))[0]) + return Vector(pt_2d.x, pt_2d.y, self.z) + @cached_property @distributionFunction def inradius(self): @@ -3137,6 +3377,10 @@ def AABB(self): xmin, ymin, xmax, ymax = self.polygons.bounds return ((xmin, ymin, self.z), (xmax, ymax, self.z)) + @property + def _sampleVals(self): + return (3,) + @distributionFunction def buffer(self, amount): buffered_polygons = self.polygons.buffer(amount) @@ -3613,20 +3857,27 @@ def end(self): roll=orientation.roll, ) + @cached + def reverse(self): + """Return a copy of this `PolylineRegion`, reversed.""" + return PolylineRegion(polyline=self.lineString.reverse()) + def defaultOrientation(self, point): start, end = self.nearestSegmentTo(point) return start.angleTo(end) def uniformPointInner(self): - pointA, pointB = random.choices( - self.segments, cum_weights=self.cumulativeLengths - )[0] - interpolation = random.random() - x, y = averageVectors(pointA, pointB, weight=interpolation) + return self.parameterizedUniformPointInner([random.random()]) + + def parameterizedUniformPointInner(self, vals): + # assert len(vals) == 1 + + pt = self.pointAlongBy(vals[0], normalized=True) + if self._usingDefaultOrientation: - return OrientedVector(x, y, 0, headingOfSegment(pointA, pointB)) + return OrientedVector(pt.x, pt.y, 0, self.defaultOrientation(pt)) else: - return self.orient(Vector(x, y, 0)) + return self.orient(pt) def containsRegionInner(self, other, tolerance): poly = toPolygon(other) @@ -3700,6 +3951,10 @@ def distanceTo(self, point) -> float: dist2D = self.lineString.distance(makeShapelyPoint(point)) return math.hypot(dist2D, point.z) + @distributionMethod + def closestPointTo(self, target): + return toVector(shapely.ops.nearest_points(self.lineString, target)[0]) + def projectVector(self, point, onDirection): raise TypeError('PolylineRegion does not support projection using "on"') @@ -3741,6 +3996,18 @@ def pointAlongBy(self, distance, normalized=False) -> Vector: pt = self.lineString.interpolate(distance, normalized=normalized) return Vector(pt.x, pt.y) + def substring(self, start, end, normalized=False): + """Compute a substring of this polyline. + + If **normalized** is true, then start and end should be between 0 and 1, and + are interpreted as a fraction of the length of the polyline. + """ + return PolylineRegion( + polyline=shapely.ops.substring( + self.lineString, start, end, normalized=normalized + ) + ) + def equallySpacedPoints(self, num): return [self.pointAlongBy(d) for d in numpy.linspace(0, self.length, num)] @@ -3764,6 +4031,10 @@ def AABB(self): xmin, ymin, xmax, ymax = self.lineString.bounds return ((xmin, ymin, 0), (xmax, ymax, 0)) + @property + def _sampleVals(self): + return (1,) + def show(self, plt, style="r-", **kwargs): plotPolygon(self.lineString, plt, style=style, **kwargs) @@ -3859,7 +4130,11 @@ def __init__(self, name, points, kdTree=None, orientation=None, tolerance=1e-6): self.tolerance = tolerance def uniformPointInner(self): - i = random.randrange(0, len(self.points)) + return self.parameterizedUniformPointInner([random.random()]) + + def parameterizedUniformPointInner(self, vals): + # assert len(vals) == 1 + i = int(vals[0] * len(self.points)) return self.orient(Vector(*self.points[i])) def intersects(self, other, triedReversed=False): @@ -3915,6 +4190,12 @@ def distanceTo(self, point): distance, _ = self.kdTree.query(point) return distance + @distributionMethod + def closestPointTo(self, target): + point = toVector(point).coordinates + _, neighbor_i = self.kdTree.query(point) + return toVector(self.points[neighbor_i]) + def projectVector(self, point, onDirection): raise TypeError('PointSetRegion does not support projection using "on"') @@ -3925,6 +4206,10 @@ def AABB(self): tuple(numpy.amax(self.points, axis=0)), ) + @property + def _sampleVals(self): + return (1,) + def __eq__(self, other): if type(other) is not PointSetRegion: return NotImplemented @@ -4039,6 +4324,9 @@ def containsRegionInner(self, reg, tolerance): def projectVector(self, point, onDirection): raise TypeError('GridRegion does not support projection using "on"') + def closestPointTo(self, target): + raise NotImplementedError + ################################################################################################### # View Regions diff --git a/src/scenic/core/requirements.py b/src/scenic/core/requirements.py index 9c29b12e8..d67fe7511 100644 --- a/src/scenic/core/requirements.py +++ b/src/scenic/core/requirements.py @@ -15,6 +15,7 @@ from scenic.core.errors import InvalidScenarioError from scenic.core.lazy_eval import needsLazyEvaluation from scenic.core.propositions import Atomic, PropositionNode +from scenic.core.utils import DefaultIdentityDict import scenic.syntax.relations as relations @@ -27,6 +28,7 @@ class RequirementType(enum.Enum): monitor = "require monitor" terminateWhen = "terminate when" terminateSimulationWhen = "terminate simulation when" + terminateAfter = "terminate after" # recorded values, which aren't requirements but are handled similarly record = "record" @@ -458,6 +460,10 @@ def falsifiedByInner(self, sample): one_time_monitor = self.proposition.create_monitor() return self.closure(sample, one_time_monitor) == rv_ltl.B4.FALSE + def evaluate(self): + # Used only for `terminate when`, etc. defined in setup blocks of subscenarios + return self.closure(DefaultIdentityDict()) + def __str__(self): if self.name: return self.name diff --git a/src/scenic/core/scenarios.py b/src/scenic/core/scenarios.py index fa93d454b..984fd108e 100644 --- a/src/scenic/core/scenarios.py +++ b/src/scenic/core/scenarios.py @@ -3,9 +3,12 @@ import dataclasses import io import itertools +import multiprocessing +import os import random import sys import time +import warnings import numpy import trimesh @@ -38,6 +41,7 @@ ) from scenic.core.sample_checking import BasicChecker, WeightedAcceptanceChecker from scenic.core.serialization import Serializer, dumpAsScenicCode +from scenic.core.utils import setSeed from scenic.core.vectors import Vector # Global params @@ -285,9 +289,7 @@ def __init__( self.egoObject = egoObject self.params = dict(params) self.externalParams = tuple(externalParams) - self.externalSampler = ExternalSampler.forParameters( - self.externalParams, self.params - ) + self.externalSampler = None self.monitors = tuple(monitors) self.behaviorNamespaces = behaviorNamespaces self.dynamicScenario = dynamicScenario @@ -315,6 +317,7 @@ def __init__( self.dependencies = ( self._instances + paramDeps + tuple(requirementDeps) + tuple(behaviorDeps) ) + self._scenarioCreationData = None # Setup the default checker self.defaultRequirements = self.generateDefaultRequirements() @@ -324,6 +327,15 @@ def setSampleChecker(self, checker): self.checker = checker self.checker.setRequirements(self.defaultRequirements + self.userRequirements) + def createExternalSampler(self, externalParams): + assert self.externalSampler is None + self.externalParams += tuple( + p for p in set(externalParams) if p not in self.externalParams + ) + self.externalSampler = ExternalSampler.forParameters( + self.externalParams, self.params + ) + def containerOfObject(self, obj): if hasattr(obj, "regionContainedIn") and obj.regionContainedIn is not None: return obj.regionContainedIn @@ -400,11 +412,26 @@ def generate(self, maxIterations=2000, verbosity=0, feedback=None): Raises: `RejectionException`: if no valid sample is found in **maxIterations** iterations. """ - scenes, iterations = self.generateBatch(1, maxIterations, verbosity, feedback) - return scenes[0], iterations + scenes, totalIterations = self.generateBatch( + numScenes=1, + maxIterations=maxIterations, + verbosity=verbosity, + feedback=feedback, + numWorkers=0, + mute=False, + ) + assert len(scenes) == 1 + return (scenes[0], totalIterations) def generateBatch( - self, numScenes, maxIterations=float("inf"), verbosity=0, feedback=None + self, + numScenes, + maxIterations=float("inf"), + verbosity=0, + feedback=None, + numWorkers=0, + mute=True, + serialized=False, ): """Sample several `Scene` objects from this scenario. @@ -416,6 +443,10 @@ def generateBatch( verbosity (int): Verbosity level. feedback (float): Feedback to pass to external samplers doing active sampling. See :mod:`scenic.core.external_params`. + numWorkers (int): The number of workers to be used when generating scenes. If numWorkers + is 0, scenes will be generated in the main process. + mute (bool): Whether or not to mute stdOut and stdErr in the worker processes. + serialized (bool): Whether or not to return scenes in a serialized format. Returns: A pair with a list of the sampled `Scene` objects and the total number @@ -424,21 +455,193 @@ def generateBatch( Raises: `RejectionException`: if not enough valid samples are found in **maxIterations** iterations. """ - totalIterations = 0 - scenes = [] + stream = self.generateStream( + numScenes=numScenes, + maxIterations=maxIterations, + verbosity=verbosity, + feedback=feedback, + numWorkers=numWorkers, + mute=mute, + serialized=serialized, + deterministic=True, + iterationCount=True, + ) + results_list = list(stream) + scenes = tuple(r[0] for r in results_list) + totalIterations = sum([r[1] for r in results_list]) + return (scenes, totalIterations) - for _ in range(numScenes): - try: - remainingIts = maxIterations - totalIterations - scene, iterations = self._generateInner(remainingIts, verbosity, feedback) - scenes.append(scene) - totalIterations += iterations - except RejectionException: - raise RejectionException( - f"failed to generate scenario in {maxIterations} iterations" + def generateStream( + self, + numScenes, + maxIterations=float("inf"), + verbosity=0, + feedback=None, + numWorkers=0, + bufferSize=None, + mute=True, + serialized=False, + deterministic=True, + iterationCount=False, + ): + """Sample a stream of `Scene` objects from this scenario. + + For a description of how scene generation is done, see `scene generation`. This function can produce + both finite and infinite streams (depending on the value of ``numScenes``). + + .. note:: + NOTE: The ``deterministic`` parameter is by default set to True, meaning that scenes + will be returned in a fixed order for a given Scenic seed. Setting this to False + means scenes will be returned in a possibly non-deterministic order, but with possibly + decreased latency. When ``deterministic`` is set to ``False``, the ordering of the returned scenes + is not fully random, with scenes that are easier to generate being more likely to be returned + earlier in the stream. Despite this, the overall distribution of the returned scenes still + matches the scenario. When generating an infinite stream, ``deterministic`` must be set to ``True``. + + Args: + numScenes (int): Number of scenes to generate, or ``float('inf')`` to sample an infinite stream + of Scenes. + maxIterations (int): Maximum number of rejection sampling iterations (over all scenes). + verbosity (int): Verbosity level. + feedback (float): Feedback to pass to external samplers doing active sampling. + See :mod:`scenic.core.external_params`. + numWorkers (int): The number of workers to be used when generating scenes. If numWorkers + is 0, scenes will be generated in the main process. + bufferSize (int): The number of scenes to have available at any given time, or `None` to + use default values. If set to ``None`` and ``numScenes`` is finite, all scenes are generated + in a greedy fashon. If set to ``None`` and ``numScenes`` is infinite, set to a default + value of ``2*numWorkers``. + mute (bool): Whether or not to mute stdOut and stdErr in the worker processes. + serialized (bool): Whether or not to return scenes in a serialized format. + deterministic (bool): Whether or not scenes will be returned in a deterministic order. This + must be set to `True` when generating an infinite stream. + iterationCount (bool): Whether or not to return the number of iterations used to generate each + scene. If this is set to `False`, the return type is simply an iterable of `Scene` objects. + + Returns: + An iterable of pairs with a sampled `Scene` and the number of iterations used for that scene + (if iterationCount is set to True). + """ + if numScenes <= 0: + raise ValueError("`numScenes` must be at least 1.") + + if not isinstance(numScenes, int) and numScenes != float("inf"): + raise ValueError("`numScenes` must be either an `int` or `float('inf')`.") + + if numScenes == float("inf") and not deterministic: + raise ValueError( + "`deterministic` must be set to `True` when generating an infinite stream." + ) + + if bufferSize is None and numScenes == float("inf"): + bufferSize = 2 * numWorkers + + if numWorkers == 0: + totalIterations = 0 + returnedResultCount = 0 + + while returnedResultCount < numScenes: + try: + remainingIts = maxIterations - totalIterations + rawScene, iterations = self._generateInner( + remainingIts, verbosity, feedback + ) + scene = self.sceneToBytes(rawScene) if serialized else rawScene + totalIterations += iterations + if iterationCount: + yield (scene, iterations) + else: + yield scene + returnedResultCount += 1 + except RejectionException: + raise RejectionException( + f"failed to generate scenario in {maxIterations} iterations" + ) + else: + if maxIterations != float("inf"): + raise ValueError("maxIterations not supported for parallel sampling.") + + if feedback is not None: + raise ValueError("Feedback not supported for parallel sampling.") + + # Initialize results tracking data + returnedResultCount = 0 + + # Initialize random generator + rand_generator = numpy.random.default_rng(random.getrandbits(32)) + + # Initialize queues + seedQueue = multiprocessing.Queue() + seedHistory = [] + putSeedCount = 0 + + def putSeed(): + nonlocal putSeedCount + newSeed = int(rand_generator.integers(2**32)) + seedQueue.put(newSeed) + if deterministic: + seedHistory.append(newSeed) + putSeedCount += 1 + + initialSeedCount = numScenes if bufferSize is None else bufferSize + for _ in range(initialSeedCount): + putSeed() + + sceneQueue = multiprocessing.Queue() + + # Initialize processes + params = (self._scenarioCreationData, seedQueue, sceneQueue, verbosity, mute) + processes = [ + multiprocessing.Process(target=generateInnerBatchHelper, args=params) + for _ in range(numWorkers) + ] + + # Initialized result management functions + resultsDict = {} + + def getResult(): + sceneBytes, resultIterations, resultSeed = sceneQueue.get() + resultScene = ( + sceneBytes + if serialized + else self.sceneFromBytes(sceneBytes, verify=False) ) + return (resultScene, resultIterations), resultSeed + + def getNextResult(): + if not deterministic: + nextResult = getResult()[0] + return nextResult if iterationCount else nextResult[0] + + while True: + if seedHistory[0] in resultsDict: + nextResult = resultsDict[seedHistory.pop(0)] + return nextResult if iterationCount else nextResult[0] + + result, resultSeed = getResult() + resultsDict[resultSeed] = result - return scenes, totalIterations + # Start sampling processes and yield samples + try: + # Start processes + for process in processes: + process.start() + + # Retrieve results + while returnedResultCount < numScenes: + yield getNextResult() + returnedResultCount += 1 + + if putSeedCount < numWorkers: + putSeed() + + finally: + # Close processes and queues + for process in processes: + process.terminate() + + seedQueue.close() + sceneQueue.close() def _generateInner(self, maxIterations, verbosity, feedback): # choose which custom requirements will be enforced for this sample @@ -753,3 +956,33 @@ def simulationFromBytes( data = io.BytesIO(data) scene = self.sceneFromBytes(data, verify=verify, allowPickle=allowPickle) return simulator.simulate(scene, replay=data, **kwargs) + + +def generateInnerBatchHelper( + scenarioCreationData, seedQueue, sceneQueue, verbosity, mute +): + if mute: + sys.stdout = open(os.devnull, "w") + sys.stderr = open(os.devnull, "w") + + from scenic.syntax.translator import _scenarioFromStream + + scenario = _scenarioFromStream( + stream=io.BytesIO(scenarioCreationData["streamLines"]), + compileOptions=scenarioCreationData["compileOptions"], + filename=scenarioCreationData["filename"], + scenario=scenarioCreationData["scenario"], + path=scenarioCreationData["path"], + _cacheImports=False, + ) + + while True: + seed = seedQueue.get() + setSeed(seed) + + scene, iterations = scenario._generateInner( + maxIterations=float("inf"), verbosity=verbosity, feedback=None + ) + sceneBytes = scenario.sceneToBytes(scene) + + sceneQueue.put((sceneBytes, iterations, seed)) diff --git a/src/scenic/core/sensors.py b/src/scenic/core/sensors.py index db15f27df..b11f0b7ae 100644 --- a/src/scenic/core/sensors.py +++ b/src/scenic/core/sensors.py @@ -101,7 +101,7 @@ class Recorder: def __init__(self): self._recording = False - def beginRecording(self, config, simulationName, timestep, globalParams): + def beginRecording(self, config, simulationName, timestep, globalParams, currentTime): assert not self._recording self._recording = True self.simulationName = simulationName @@ -121,9 +121,10 @@ def beginRecording(self, config, simulationName, timestep, globalParams): assert val >= 0, val if unit == "steps": assert isinstance(val, int), val - self._delay = val + delay = val else: # unit == "seconds" - self._delay = max(0, math.floor(val / timestep)) + delay = max(0, math.floor(val / timestep)) + self._startTime = currentTime + delay def recordValue(self, value, step): raise NotImplementedError @@ -133,7 +134,8 @@ def endRecording(self, canceled): self._recording = False def _record(self, value, step): - if step >= self._delay and step % self._period == 0: + relativeTime = step - self._startTime + if relativeTime >= 0 and relativeTime % self._period == 0: self.recordValue(np.asarray(value), step) @staticmethod @@ -264,7 +266,7 @@ def videoHandler(path, values, timestep, options): @fileHandler("npz") def npzHandler(path, values, timestep, options): - timesteps, values = zip(*values) + timesteps, values = zip(*values) if values else ([], []) np.savez_compressed(path, timesteps=timesteps, values=values) diff --git a/src/scenic/core/serialization.py b/src/scenic/core/serialization.py index 580ffb32b..97c083734 100644 --- a/src/scenic/core/serialization.py +++ b/src/scenic/core/serialization.py @@ -8,12 +8,15 @@ import hashlib 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 def deterministicHash(mapping, *, digest_size=8): @@ -392,3 +395,224 @@ def readStr(stream): Serializer.addCodec(str, writeStr, readStr) + + +def toOpenScenario( + simulationResult, + scenario, + scene, + mapPath=None, + scenarioName="ScenicScenario", +): + """Export a `SimulationResult` as a `scenariogeneration.xosc.scenario `_ object. + + Args: + simulationResult: The `SimulationResult` to be exported to XOSC + scenario: The scenario from which simulationResult was sampled. + scene: The scene from which simulationResult was sampled. + mapPath: The path to the XODR map used to run the simulation. If + one is not provided the `map` param of the scenario is used. + scenarioName: The name of the scenario in the generated XOSC file. + """ + try: + import scenariogeneration + from scenariogeneration import ScenarioGenerator, xosc + except ModuleNotFoundError as e: + raise ModuleNotFoundError( + "The `scenariogeneration` package is required to use Scenic's XOSC export functionality." + ) from e + + # Create catalog + xosc_catalog = xosc.Catalog() + + # Create parameters + xosc_paramdec = xosc.ParameterDeclarations() + + # Extract map + if mapPath is None: + if "map" not in scenario.params: + raise ValueError( + "No `mapPath` provided and scenario does not have a `map` parameter defined." + ) + mapPath = ( + mapPath if mapPath is not None else os.path.abspath(scenario.params["map"]) + ) + xosc_road = xosc.RoadNetwork(roadfile=mapPath) + + # 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( + obj.maxSteeringAngle, + obj.wheelDiameter, + obj.trackWidth, + obj.wheelbase, + obj.groundClearance, + ) + veh_ra = xosc.Axle( + obj.maxSteeringAngle, + obj.wheelDiameter, + obj.trackWidth, + 0, + obj.groundClearance, + ) + xosc_obj = xosc.Vehicle( + name=obj_name, + vehicle_type=xosc.VehicleCategory.car, + boundingbox=veh_bb, + frontaxle=veh_fa, + rearaxle=veh_ra, + max_speed=obj.maxSpeed, + max_acceleration=obj.maxAcceleration, + max_deceleration=obj.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=obj.mass, + boundingbox=ped_bb, + category=xosc.PedestrianCategory.pedestrian, + model=None, + role=None, + ) + else: + warnings.warn( + f"Object {obj} of unsupported type is being ignored during XOSC export." + ) + continue + + xosc_objects[obj] = xosc_obj + entities.add_scenario_object(obj_name, xosc_obj) + + # Helper function + def pos_to_WorldPosition(obj, pos, yaw): + # XOSC Reference point is back axle, so we must translate Scenic's + # convention to this. + state_position = ( + pos.offsetRotated(yaw, Vector(0, -0.5 * obj.wheelbase, 0)) + if obj.isVehicle + else pos + ) + state_orientation = yaw + math.radians(90) + return xosc.WorldPosition( + x=state_position.x, + y=state_position.y, + z=state_position.z, + h=state_orientation, + ) + + # Initial states + init = xosc.Init() + + for obj, xosc_obj in xosc_objects.items(): + obj_init_action = xosc.TeleportAction( + pos_to_WorldPosition(obj, obj.position, obj.yaw) + ) + 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): + action_positions.append( + pos_to_WorldPosition( + obj, states.positions[obj_i], states.orientations[obj_i].yaw + ) + ) + action_times.append(simulationResult.timestep * t) + + 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..39b32ebbf 100644 --- a/src/scenic/core/simulators.py +++ b/src/scenic/core/simulators.py @@ -11,12 +11,21 @@ """ import abc -from collections import defaultdict +from collections import OrderedDict, defaultdict +from contextlib import contextmanager import enum +import io import math +import multiprocessing import numbers +import os +import random +import sys import time import types +import warnings + +import numpy from scenic.core.distributions import RejectionException from scenic.core.dynamics import GuardViolation, RejectSimulationException @@ -31,6 +40,7 @@ ) from scenic.core.requirements import RequirementType from scenic.core.serialization import Serializer +from scenic.core.utils import setSeed from scenic.core.vectors import Vector @@ -138,9 +148,8 @@ def simulate( (rarely) and its security implications. Returns: - A `Simulation` object representing the completed simulation, or `None` if no - simulation satisfying the requirements could be found within - **maxIterations** iterations. + An initialized simulation, or `None` if no simulation satisfying + the requirements could be found within **maxIterations** iterations. Raises: SimulationCreationError: if an error occurred while trying to run a @@ -198,6 +207,53 @@ def simulate( ) return simulation + @contextmanager + def simulateStepped( + self, + scene, + maxSteps=None, + *, + name="SteppedSimulation", + timestep=None, + verbosity=None, + replay=None, + enableReplay=True, + enableDivergenceCheck=False, + divergenceTolerance=0, + continueAfterDivergence=False, + allowPickle=False, + ): + if self._destroyed: + raise RuntimeError( + "simulator cannot run additional simulations " + "(the destroy() method has already been called)" + ) + if verbosity is None: + verbosity = errors.verbosityLevel + + simulation = self.createSimulation( + scene, + maxSteps=maxSteps, + name=name, + verbosity=verbosity, + timestep=timestep, + replay=replay, + enableReplay=enableReplay, + enableDivergenceCheck=enableDivergenceCheck, + divergenceTolerance=divergenceTolerance, + continueAfterDivergence=continueAfterDivergence, + allowPickle=allowPickle, + ) + try: + yield simulation + except (RejectSimulationException, RejectionException, GuardViolation) as e: + # This simulation will be thrown out, but attach it to the exception + # to aid in debugging. + e.simulation = self + raise + finally: + simulation.cleanup() + def replay(self, scene, replay, **kwargs): """Replay a simulation. @@ -214,13 +270,14 @@ def _runSingleSimulation( if verbosity >= 2: print(f" Starting simulation {name}...") try: - simulation = self.createSimulation( + with self.simulateStepped( scene, maxSteps=maxSteps, name=name, verbosity=verbosity, **kwargs, - ) + ) as simulation: + simulation._run() except (RejectSimulationException, RejectionException, GuardViolation) as e: if verbosity >= 2: print( @@ -347,11 +404,14 @@ def __init__( self.currentTime = 0 self.timestep = 1 if timestep is None else float(timestep) self.verbosity = verbosity + self.maxSteps = maxSteps self.name = name self.worker_num = 0 self.actionSequence = [] + self._cleaned = False + # Prepare to save or load a replay. self.initializeReplay(replay, enableReplay, enableDivergenceCheck, allowPickle) self.divergenceTolerance = divergenceTolerance @@ -364,170 +424,204 @@ def __init__( import scenic.syntax.veneer as veneer veneer.beginSimulation(self) - dynamicScenario = self.scene.dynamicScenario + self.dynamicScenario = self.scene.dynamicScenario # Create objects and perform simulator-specific initialization. self.setup() # Initialize the top-level dynamic scenario. - dynamicScenario._start() + self.dynamicScenario._start() # Update all objects in case the simulator has adjusted any dynamic # properties during setup. self.updateObjects() - # Run the simulation. - terminationType, terminationReason = self._run(dynamicScenario, maxSteps) - - # Stop all remaining scenarios. - # (and reject if some 'require eventually' condition was never satisfied) - for scenario in tuple(reversed(veneer.runningScenarios)): - scenario._stop("simulation terminated") - - # Record finally-recorded values. - values = dynamicScenario._evaluateRecordedExprs( - RequirementType.recordFinal, self.currentTime - ) - for name, val in values.items(): - self.records[name] = val - - # Package up simulation results into a compact object. - result = SimulationResult( - self.trajectory, - self.actionSequence, - terminationType, - terminationReason, - self.records, - ) - self.result = result - except (RejectSimulationException, RejectionException, GuardViolation) as e: + except Exception as e: # This simulation will be thrown out, but attach it to the exception # to aid in debugging. + self.cleanup() e.simulation = self raise - finally: - self.destroy() - for obj in self.objects: - disableDynamicProxyFor(obj) - for agent in self.agents: - if agent.behavior and agent.behavior._isRunning: - agent.behavior._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(reversed(veneer.runningScenarios)): - scenario._stop("exception", quiet=True) - veneer.endSimulation(self) - - def _run(self, dynamicScenario, maxSteps): + + def _run(self): assert self.currentTime == 0 while True: - if self.verbosity >= 3: - print(f" Time step {self.currentTime}:") - - # Run compose blocks of compositional scenarios - # (and check if any requirements defined therein fail) - # N.B. if the top-level scenario completes, we don't immediately end - # the simulation since we need to check if any monitors reject first. - terminationReason = dynamicScenario._step() - terminationType = TerminationType.scenarioComplete - - # Update observations of objects with sensors - for obj in self.objects: - if not obj.sensors: - continue - obj.observations.update( - {key: sensor.getObservation() for key, sensor in obj.sensors.items()} - ) + self.advance() + + if self.result: + return + + def advance(self): + if self.result or self._cleaned: + raise TerminatedSimulationException() + + if self.verbosity >= 3: + print(f" Time step {self.currentTime}:") + + # Run compose blocks of compositional scenarios + # (and check if any requirements defined therein fail) + # N.B. if the top-level scenario completes, we don't immediately end + # the simulation since we need to check if any monitors reject first. + terminationReason = self.dynamicScenario._step() + terminationType = TerminationType.scenarioComplete + + # Update observations of objects with sensors + for obj in self.objects: + if not obj.sensors: + continue + obj.observations.update( + {key: sensor.getObservation() for key, sensor in obj.sensors.items()} + ) + + # Record current state of the simulation + self._recordCurrentState() - # Record current state of the simulation - self.recordCurrentState() + # Run monitors + newReason = self.dynamicScenario._runMonitors() + if newReason is not None: + terminationReason = newReason + terminationType = TerminationType.terminatedByMonitor - # Run monitors - newReason = dynamicScenario._runMonitors() - if newReason is not None: - terminationReason = newReason - terminationType = TerminationType.terminatedByMonitor + # Check if users manually closed out display for simulator + if "Dead" in str(self.screen): + return ( + TerminationType.terminatedByUser, + "user manually terminated simulation", + ) - # Check if users manually closed out display for simulator - if "Dead" in str(self.screen): - return ( - TerminationType.terminatedByUser, - "user manually terminated simulation", + # "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: + return self.terminateSimulation(terminationType, terminationReason) + terminationReason = self.dynamicScenario._checkSimulationTerminationConditions() + if terminationReason is not None: + if terminationReason.ty is RequirementType.terminateAfter: + return self.terminateSimulation( + TerminationType.timeLimit, terminationReason + ) + else: + return self.terminateSimulation( + TerminationType.simulationTerminationCondition, + terminationReason, ) + if self.maxSteps and self.currentTime >= self.maxSteps: + return self.terminateSimulation( + TerminationType.timeLimit, f"reached time limit ({self.maxSteps} steps)" + ) - # "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: - return terminationType, terminationReason - terminationReason = dynamicScenario._checkSimulationTerminationConditions() - if terminationReason is not None: - return TerminationType.simulationTerminationCondition, terminationReason - if maxSteps and self.currentTime >= maxSteps: - return TerminationType.timeLimit, f"reached time limit ({maxSteps} steps)" - - # Clear lastActions for all objects - for obj in self.objects: - obj.lastActions = tuple() - - # Update agents with any objects that now have behaviors (and are not already agents) - self.agents += [ - obj for obj in self.objects if obj.behavior and obj not in self.agents - ] - - # Compute the actions of the agents in this time step - allActions = defaultdict(tuple) - schedule = self.scheduleForAgents() - if not set(self.agents) == set(schedule): - raise RuntimeError("Simulator schedule does not contain all agents") - for agent in schedule: - # If agent doesn't have a behavior right now, continue - if not agent.behavior: - continue - - # Run the agent's behavior to get its actions - actions = agent.behavior._step() - - # Handle pseudo-actions marking the end of a simulation/scenario - if isinstance(actions, _EndSimulationAction): - return TerminationType.terminatedByBehavior, str(actions) - elif isinstance(actions, _EndScenarioAction): - scenario = actions.scenario - if scenario._isRunning: - scenario._stop(actions) - if scenario is dynamicScenario: - # Top-level scenario was terminated, so whole simulation will end. - return TerminationType.terminatedByBehavior, str(actions) - actions = () - - # Check ordinary actions for compatibility - 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 action(s) {actions}" + # Clear lastActions for all objects + for obj in self.objects: + obj.lastActions = tuple() + + # Update agents with any objects that now have behaviors (and are not already agents) + self.agents += [ + obj for obj in self.objects if obj.behavior and obj not in self.agents + ] + + # Compute the actions of the agents in this time step + allActions = defaultdict(tuple) + schedule = self.scheduleForAgents() + if not set(self.agents) == set(schedule): + raise RuntimeError("Simulator schedule does not contain all agents") + for agent in schedule: + # If agent doesn't have a behavior right now, continue + if not agent.behavior: + continue + + # Run the agent's behavior to get its actions + actions = agent.behavior._step() + if actions is None: + actions = tuple() + + # Handle pseudo-actions marking the end of a simulation/scenario + if isinstance(actions, _EndSimulationAction): + return self.terminateSimulation( + TerminationType.terminatedByBehavior, str(actions) + ) + elif isinstance(actions, _EndScenarioAction): + scenario = actions.scenario + if scenario._isRunning: + scenario._stop(actions) + if scenario is self.dynamicScenario: + # Top-level scenario was terminated, so whole simulation will end. + return self.terminateSimulation( + TerminationType.terminatedByBehavior, str(actions) ) + actions = () + + # Check ordinary actions for compatibility + 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 action(s) {actions}" + ) + + # Save actions for execution below + allActions[agent] = actions - # Save actions for execution below - allActions[agent] = actions + # Log lastActions + agent.lastActions = actions - # Log lastActions + # Execute the actions + if self.verbosity >= 3: + for agent, actions in allActions.items(): + print(f" Agent {agent} takes action(s) {actions}") agent.lastActions = actions + self.actionSequence.append(allActions) + self.executeActions(allActions) + + # Run the simulation for a single step and read its state back into Scenic + self.step() + self.currentTime += 1 + self.updateObjects() + + def terminateSimulation(self, terminationType, terminationReason): + import scenic.syntax.veneer as veneer + + # Stop all remaining scenarios (and handle their `record final` statements; + # also reject if some `require eventually` condition was never satisfied). + for scenario in tuple(reversed(veneer.runningScenarios)): + scenario._stop("simulation terminated") + + # Package up simulation results into a compact object. + result = SimulationResult( + self.name, + self.trajectory, + self.actionSequence, + terminationType, + terminationReason, + self.records, + ) + self.result = result - # Execute the actions - if self.verbosity >= 3: - for agent, actions in allActions.items(): - print(f" Agent {agent} takes action(s) {actions}") - self.actionSequence.append(allActions) - self.executeActions(allActions) + self.cleanup() - # Run the simulation for a single step and read its state back into Scenic - self.step() - self.currentTime += 1 - self.updateObjects() + def cleanup(self): + # No need to repeat cleanup if we've already done it + if self._cleaned: + return + + # Remember that we have cleaned up. + self._cleaned = True + + import scenic.syntax.veneer as veneer + + self.destroy() + for obj in self.objects: + disableDynamicProxyFor(obj) + for agent in self.agents: + if agent.behavior and agent.behavior._isRunning: + agent.behavior._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(reversed(veneer.runningScenarios)): + scenario._stop("exception", quiet=True) + veneer.endSimulation(self) def setup(self): """Set up the simulation to run in the simulator. @@ -596,25 +690,18 @@ def createObjectInSimulator(self, obj): """ raise NotImplementedError - def recordCurrentState(self): - dynamicScenario = self.scene.dynamicScenario - records = self.records + def _recordCurrentState(self): + # Record values of `record initial` and `record` statements. + # (calls _record and _recordTimeSeries below) + self.scene.dynamicScenario._updateRecords() - # Record initially-recorded values - step = self.currentTime - if step == 0: - values = dynamicScenario._evaluateRecordedExprs( - RequirementType.recordInitial, step - ) - for name, val in values.items(): - records[name] = val + self.trajectory.append(self.currentState()) - # Record time-series values - values = dynamicScenario._evaluateRecordedExprs(RequirementType.record, step) - for name, val in values.items(): - records[name].append((self.currentTime, val)) + def _record(self, name, value): + self.records[name] = value - self.trajectory.append(self.currentState()) + def _recordTimeSeries(self, name, value): + self.records[name].append((self.currentTime, value)) def replayCanContinue(self): if not self.replaying: @@ -811,7 +898,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): @@ -922,6 +1021,7 @@ class SimulationResult: """Result of running a simulation. Attributes: + name: Name of the simulation, if any trajectory: A tuple giving for each time step the simulation's 'state': by default the positions of every object. See `Simulation.currentState`. finalState: The last 'state' of the simulation, as above. @@ -934,7 +1034,10 @@ class SimulationResult: values its expression took during the simulation. """ - def __init__(self, trajectory, actions, terminationType, terminationReason, records): + def __init__( + self, name, trajectory, actions, terminationType, terminationReason, records + ): + self.name = name self.trajectory = tuple(trajectory) assert self.trajectory self.finalState = self.trajectory[-1] @@ -942,3 +1045,264 @@ def __init__(self, trajectory, actions, terminationType, terminationReason, reco self.terminationType = terminationType self.terminationReason = str(terminationReason) self.records = dict(records) + + +class TerminatedSimulationException(Exception): + pass + + +class SimulatorGroup: + """A group of simulators for running parallel simulations. + + Args: + numWorkers: Number of workers in this group. + simulatorClass: The simulator class this group is composed of. + simulatorParams: An optional single or list of kwarg dictionaries to be passed as parameters + when creating the simulators in this group. If simulatorParams is a list of dicts, + ``len(simulatorParams)`` should equal ``numWorkers``. + bufferSize: An optional integer indicating the size of the job buffer. If ``None``, the value is + set to ``2 * numWorkers``. + mute: Whether or not to mute stdOut and stdErr in the worker processes. + returnFinalState: Whether or not returned `SimulationResult` objects should contain the ``finalState`` + property. Set to ``False`` by default to minimize overhead. + returnTrajectory: Whether or not returned `SimulationResult` objects should contain the ``trajectry`` + property. Set to ``False`` by default to minimize overhead. + """ + + def __init__( + self, + numWorkers, + simulatorClass, + simulatorParams=None, + bufferSize=None, + mute=True, + returnFinalState=False, + returnTrajectory=False, + ): + if numWorkers <= 0: + raise ValueError("`numWorkers` must be at least 1.") + self.numWorkers = numWorkers + self.simulatorClass = simulatorClass + simulatorParams = simulatorParams if simulatorParams else dict() + if isinstance(simulatorParams, dict): + self.simulatorParams = [simulatorParams for _ in range(numWorkers)] + elif isinstance(simulatorParams, collections.abc.Iterable): + if len(simulatorParams) != numWorkers: + raise ValueError( + "Length of `simulatorParams` does not match `numWorkers`." + ) + self.simulatorParams = tuple(simulatorParams) + else: + raise ValueError("`simulatorParams` is not a dict or iterable of dicts.") + self.bufferSize = 2 * numWorkers if bufferSize is None else bufferSize + if self.bufferSize <= 1: + raise ValueError("`bufferSize` must be at least 1.") + self.mute = mute + self.returnFinalState = returnFinalState + self.returnTrajectory = returnTrajectory + + def _jobName(self, jobId): + return f"Scene{jobId}" + + def _serializeScene(self, scene, scenario, serialized): + from scenic.core.scenarios import Scene + + if serialized: + if not isinstance(scene, bytes): + raise ValueError( + f"Scene provided has type `{type(scene)}` instead of type `bytes`, but serialized was set to True." + ) + return scene + else: + if not isinstance(scene, Scene): + raise ValueError( + f"Scene provided has type `{type(scene)}` instead of type `Scene`, but serialized was set to False." + ) + return scenario.sceneToBytes(scene) + + def _prepareJob( + self, scene, simulateParams, jobId, scenario, serialized, rand_generator + ): + serializedScene = self._serializeScene(scene, scenario, serialized) + jobParams = simulateParams.copy() + + if "name" in jobParams: + warnings.warn( + "`name` in `simulateParams` is ignored and overwritten by custom name when using `SimulatorGroup`." + ) + jobName = self._jobName(jobId) + jobParams["name"] = jobName + + seed = int(rand_generator.integers(2**32)) + + return (jobId, serializedScene, jobParams, seed) + + def simulateBatch(self, scenario, scenes, simulateParams=None, serialized=True): + """Simulate and return a batch of `SimulationResult` objects. + + Args: + scenario: The scenario that the scenes are sampled from. + scenes: An iterator of `Scene` objects sampled from ``scenario``. + simulateParams: An optional dictionary of params to be passed to simulate internally. + serialized: Whether or not ``scenes`` contains serialized scenes. + """ + return tuple( + v[1] + for v in self.simulateStream( + scenario, scenes, simulateParams, serialized, deterministic=True + ) + ) + + def simulateStream( + self, scenario, scenes, simulateParams=None, serialized=True, deterministic=False + ): + """Generate a stream of `SimulationResult` objects. + + Args: + scenario: The scenario that the scenes are sampled from. + scenes: An iterator of `Scene` objects sampled from ``scenario``. + simulateParams: An optional dictionary of params to be passed to simulate internally. + serialized: Whether or not ``scenes`` contains serialized scenes. + deterministic: Whether or not results should be returned in a deterministic order. Setting + this to ``False`` can result in decreased latency when accessing results, but the order + will not be fixed. + """ + scenes = iter(scenes) + simulateParams = simulateParams if simulateParams else dict() + + # Create helper parameters + scenarioCreationData = scenario._scenarioCreationData + jobQueue = multiprocessing.Queue() + resultQueue = multiprocessing.Queue() + + # Initialize random generator + rand_generator = numpy.random.default_rng(random.getrandbits(32)) + + # Initialize processes + processes = [] + for simulatorParams in self.simulatorParams: + params = ( + scenarioCreationData, + self.simulatorClass, + simulatorParams, + jobQueue, + resultQueue, + self.mute, + self.returnFinalState, + self.returnTrajectory, + ) + processes.append( + multiprocessing.Process(target=simulatorGroupHelper, args=params) + ) + + # Job creation utilities + remainingJobs = 0 + jobId = 0 + + def putJob(scene): + nonlocal jobId + nonlocal remainingJobs + preparedJob = self._prepareJob( + scene, + simulateParams, + jobId, + scenario, + serialized, + rand_generator=rand_generator, + ) + jobQueue.put(preparedJob) + jobId += 1 + remainingJobs += 1 + + # Initialized result management functions + lastReturnedJob = 0 + resultsDict = {} + + def getNextResult(): + if not deterministic: + return resultQueue.get() + + nonlocal lastReturnedJob + while True: + if lastReturnedJob in resultsDict: + returnResult = (lastReturnedJob, resultsDict.pop(lastReturnedJob)) + lastReturnedJob += 1 + return returnResult + + jobId, nextResult = resultQueue.get() + resultsDict[jobId] = nextResult + + try: + # Start processes + for process in processes: + process.start() + + # Initially saturate job buffer + for _ in range(self.bufferSize): + if scene := next(scenes, None): + putJob(scene) + + # Retrieve results and replenish buffer + while remainingJobs: + simulationResult = getNextResult() + remainingJobs -= 1 + + if scene := next(scenes, None): + putJob(scene) + + yield simulationResult + + finally: + # Close processes and queues + for process in processes: + process.terminate() + + jobQueue.close() + resultQueue.close() + + +def simulatorGroupHelper( + scenarioCreationData, + simulatorClass, + simulatorParams, + jobQueue, + resultQueue, + mute, + returnFinalState, + returnTrajectory, +): + if mute: + sys.stdout = open(os.devnull, "w") + sys.stderr = open(os.devnull, "w") + + from scenic.syntax.translator import _scenarioFromStream + + scenario = _scenarioFromStream( + stream=io.BytesIO(scenarioCreationData["streamLines"]), + compileOptions=scenarioCreationData["compileOptions"], + filename=scenarioCreationData["filename"], + scenario=scenarioCreationData["scenario"], + path=scenarioCreationData["path"], + _cacheImports=False, + ) + + simulator = simulatorClass(**simulatorParams) + + while True: + jobId, serializedScene, simulateParams, seed = jobQueue.get() + setSeed(seed) + + scene = scenario.sceneFromBytes(serializedScene, verify=False) + simulation = simulator.simulate(scene, **simulateParams) + + if simulation: + simulationResult = simulation.result + simulationResult.actions = None + if not returnFinalState: + simulationResult.finalState = None + if not returnTrajectory: + simulationResult.trajectory = None + else: + simulationResult = None + + resultQueue.put((jobId, simulationResult)) diff --git a/src/scenic/core/utils.py b/src/scenic/core/utils.py index 9817843f5..f01d58b2c 100644 --- a/src/scenic/core/utils.py +++ b/src/scenic/core/utils.py @@ -4,9 +4,12 @@ import collections from contextlib import contextmanager import functools +import io import itertools import math +import multiprocessing import os +import random import signal from subprocess import CalledProcessError import sys @@ -393,3 +396,9 @@ def get_type_hints(obj, globalns=None, localns=None): wrapped = wrapped.__wrapped__ globalns = getattr(wrapped, "__globals__", {}) return typing.get_type_hints(obj, globalns, localns) + + +def setSeed(seed): + """Set the random seed used by Scenic""" + random.seed(seed) + numpy.random.seed(seed) diff --git a/src/scenic/core/vectors.py b/src/scenic/core/vectors.py index c4ae11f48..a482babcb 100644 --- a/src/scenic/core/vectors.py +++ b/src/scenic/core/vectors.py @@ -16,7 +16,7 @@ import numpy from scipy.spatial.transform import Rotation -import shapely.geometry +import shapely from scenic.core.distributions import ( Distribution, @@ -41,6 +41,7 @@ canCoerceType, coerceToFloat, toOrientation, + toVector, ) from scenic.core.utils import argsToString, cached_property @@ -55,6 +56,8 @@ def toVector(self): class VectorOperatorDistribution(VectorDistribution): + _deterministic = True + """Vector version of OperatorDistribution.""" def __init__(self, operator, obj, operands): @@ -449,7 +452,9 @@ def toVector(self) -> Vector: @staticmethod def _canCoerceType(ty): - return issubclass(ty, (tuple, list, numpy.ndarray)) or hasattr(ty, "toVector") + return issubclass(ty, (tuple, list, numpy.ndarray, shapely.Point)) or hasattr( + ty, "toVector" + ) @staticmethod def _coerce(thing) -> Vector: @@ -460,6 +465,8 @@ def _coerce(thing) -> Vector: "expected 2D/3D vector, got " f"{type(thing).__name__} of length {l}" ) return Vector(*thing) + elif isinstance(thing, shapely.Point): + return Vector(*thing.coords[0]) else: return thing.toVector() @@ -702,6 +709,26 @@ def __getitem__(self, pos) -> Orientation: val, f"value function of {self.name} returned non-orientation" ) + @distributionMethod + def followFromTrajectory(self, pos, dist, steps=None, stepSize=None): + """Follow the field from a point for a given distance, returning intermediate points. + + Uses the forward Euler approximation, covering the given distance with + equal-size steps. The number of steps can be given manually, or computed + automatically from a desired step size. + + Arguments: + pos (`Vector`): point to start from. + dist (float): distance to travel. + steps (int): number of steps to take, or :obj:`None` to compute the number of + steps based on the distance (default :obj:`None`). + stepSize (float): length used to compute how many steps to take, or + :obj:`None` to use the field's default step size. + """ + return self._followFromHelper( + toVector(pos), dist, steps, stepSize, allPoints=True + ) + @vectorDistributionMethod def followFrom(self, pos, dist, steps=None, stepSize=None): """Follow the field from a point for a given distance. @@ -718,6 +745,16 @@ def followFrom(self, pos, dist, steps=None, stepSize=None): stepSize (float): length used to compute how many steps to take, or :obj:`None` to use the field's default step size. """ + return self._followFromHelper( + toVector(pos), dist, steps, stepSize, allPoints=False + ) + + def _followFromHelper(self, pos, dist, steps, stepSize, allPoints): + if allPoints: + from scenic.core.regions import PolylineRegion + + pts = [] + if steps is None: steps = self.minSteps stepSize = self.defaultStepSize if stepSize is None else stepSize @@ -729,8 +766,10 @@ def followFrom(self, pos, dist, steps=None, stepSize=None): for i in range(steps): rot = self[pos].getRotation() pos += rot.apply(step) + if allPoints: + pts.append(Vector(*pos)) - return Vector(*pos) + return PolylineRegion(points=pts) if allPoints else Vector(*pos) @staticmethod def forUnionOf(regions, tolerance=0): diff --git a/src/scenic/core/workspaces.py b/src/scenic/core/workspaces.py index 8daec9aab..63ffc4fa0 100644 --- a/src/scenic/core/workspaces.py +++ b/src/scenic/core/workspaces.py @@ -92,6 +92,9 @@ def scenicToSchematicCoords(self, coords): def uniformPointInner(self): return self.region.uniformPointInner() + def parameterizedUniformPointInner(self): + return self.region.parameterizedUniformPointInner() + def intersect(self, other, triedReversed=False): return self.region.intersect(other, triedReversed) @@ -117,12 +120,19 @@ def distanceTo(self, point): return self.region.distanceTo(point) def projectVector(self, point, onDirection): - raise self.region.projectVector(point, onDirection) + return self.region.projectVector(point, onDirection) + + def closestPointTo(self, target): + return self.region.closestPointTo(target) @property def AABB(self): return self.region.AABB + @property + def _sampleVals(self): + return self.region._sampleVals + @property def dimensionality(self): return self.region.dimensionality diff --git a/src/scenic/domains/driving/actions.py b/src/scenic/domains/driving/actions.py index 7f99cce1a..6d61c27db 100644 --- a/src/scenic/domains/driving/actions.py +++ b/src/scenic/domains/driving/actions.py @@ -15,7 +15,10 @@ import math +import numpy as np + from scenic.core.simulators import Action +from scenic.core.type_support import toOrientation from scenic.core.vectors import Vector ## Mixin classes indicating support for various types of actions. @@ -53,12 +56,10 @@ class Walks: """ def setWalkingDirection(self, heading): - velocity = Vector(0, self.speed).rotatedBy(heading) - self.setVelocity(velocity) + self.setOrientation(toOrientation(heading)) def setWalkingSpeed(self, speed): - velocity = speed * self.velocity.normalized() - self.setVelocity(velocity) + self.setVelocity(Vector(0, speed).rotatedBy(self.heading)) ## Actions available to all agents @@ -106,6 +107,16 @@ def applyTo(self, obj, sim): obj.setVelocity(vel) +class SetOrientationAction(Action): + """Set the orientation of an agent.""" + + def __init__(self, orientation): + self.orientation = toOrientation(orientation) + + def applyTo(self, obj, sim): + obj.setOrientation(self.orientation) + + ## Actions available to vehicles which can steer @@ -241,11 +252,8 @@ def __init__( brake = min(abs(throttle), max_brake) # Steering regulation: changes cannot happen abruptly, can't steer too much. - - if steer > past_steer + 0.1: - steer = past_steer + 0.1 - elif steer < past_steer - 0.1: - steer = past_steer - 0.1 + if past_steer is not None: + steer = np.clip(steer, past_steer - 0.1, past_steer + 0.1) if steer >= 0: steer = min(max_steer, steer) diff --git a/src/scenic/domains/driving/behaviors/__init__.py b/src/scenic/domains/driving/behaviors/__init__.py new file mode 100644 index 000000000..9a5d0c77d --- /dev/null +++ b/src/scenic/domains/driving/behaviors/__init__.py @@ -0,0 +1,2 @@ +from scenic.domains.driving.behaviors.steers import * +from scenic.domains.driving.behaviors.walks import * diff --git a/src/scenic/domains/driving/behaviors.scenic b/src/scenic/domains/driving/behaviors/steers.scenic similarity index 57% rename from src/scenic/domains/driving/behaviors.scenic rename to src/scenic/domains/driving/behaviors/steers.scenic index 173500190..e64aca8ef 100644 --- a/src/scenic/domains/driving/behaviors.scenic +++ b/src/scenic/domains/driving/behaviors/steers.scenic @@ -4,13 +4,17 @@ These behaviors are automatically imported when using the driving domain. """ import math +from abc import ABC, abstractmethod +import warnings +import shapely +from shapely.geometry import GeometryCollection, MultiPolygon, Polygon, MultiLineString, LineString, MultiPoint, Point as ShapelyPoint + +from scenic.core.regions import toShapely +from scenic.core.type_support import toVector from scenic.domains.driving.actions import * import scenic.domains.driving.model as _model -from scenic.domains.driving.roads import ManeuverType - -def concatenateCenterlines(centerlines=[]): - return PolylineRegion.unionAll(centerlines) +from scenic.domains.driving.roads import ManeuverType, Lane behavior ConstantThrottleBehavior(x): while True: @@ -25,23 +29,59 @@ behavior DriveAvoidingCollisions(target_speed=25, avoidance_threshold=10): behavior AccelerateForwardBehavior(): take SetReverseAction(False), SetHandBrakeAction(False), SetThrottleAction(0.5) -behavior WalkForwardBehavior(): - """Walk forward behavior for pedestrians. - - It will uniformly randomly choose either end of the sidewalk that the pedestrian is on, and have the pedestrian walk towards the endpoint. - """ - current_sidewalk = _model.network.sidewalkAt(self.position) - end_point = Uniform(*current_sidewalk.centerline.points) - end_vec = end_point[0] @ end_point[1] - normal_vec = Vector.normalized(end_vec) - take WalkTowardsAction(goal_position=normal_vec), SetSpeedAction(speed=1) - behavior ConstantThrottleBehavior(x): take SetThrottleAction(x) -behavior FollowLaneBehavior(target_speed = 10, laneToFollow=None, is_oppositeTraffic=False): +def getFollowLanePath(obj, minPathDistance, preferStraight, laneToFollow=None, path_metadata=None): + import shapely + import itertools + + if laneToFollow is None: + laneToFollow = obj.lane + elif not isinstance(laneToFollow, Lane): + raise ValueError("`laneToFollow` is not a `Lane`.") + + if path_metadata is None: + current_lane = laneToFollow + initial_path = current_lane.centerline.lineString + else: + current_lane = path_metadata[0] + initial_path = path_metadata[1] + + assert isinstance(initial_path, shapely.geometry.LineString) + + def mergeLineStrings(geoms): + return shapely.geometry.LineString(itertools.chain.from_iterable(geom.coords for geom in geoms)) + + ego_pt = shapely.geometry.Point(*obj.position) + path = shapely.ops.substring(initial_path, initial_path.project(ego_pt), initial_path.length) + + while path.length < minPathDistance: + straight_manuevers = [m for m in current_lane.maneuvers if m.type == ManeuverType.STRAIGHT] + + if preferStraight and straight_manuevers: + target_maneuver = Uniform(*straight_manuevers) + else: + if len(current_lane.maneuvers) > 0: + target_maneuver = Uniform(*current_lane.maneuvers) + else: + # No more maneuvers. Raise a warning and return centerline as is + warnings.warn("Could not generate path of desired distance. Returning path as is.") + break + + if target_maneuver.connectingLane != None: + path = mergeLineStrings([path, target_maneuver.connectingLane.centerline.lineString, target_maneuver.endLane.centerline.lineString]) + else: + path = mergeLineStrings([path, target_maneuver.endLane.centerline.lineString]) + + current_lane = target_maneuver.endLane + + assert isinstance(path, shapely.geometry.LineString) + return PolylineRegion(polyline=path), (current_lane, path) + +behavior FollowLaneBehavior(target_speed=10, laneToFollow=None, preferStraight=True): """ - Follow's the lane on which the vehicle is at, unless the laneToFollow is specified. + Follows the lane on which the vehicle is at, unless the laneToFollow is specified. Once the vehicle reaches an intersection, by default, the vehicle will take the straight route. If straight route is not available, then any availble turn route will be taken, uniformly randomly. If turning at the intersection, the vehicle will slow down to make the turn, safely. @@ -50,107 +90,103 @@ behavior FollowLaneBehavior(target_speed = 10, laneToFollow=None, is_oppositeTra e.g. do FollowLaneBehavior() until ... :param target_speed: Its unit is in m/s. By default, it is set to 10 m/s - :param laneToFollow: If the lane to follow is different from the lane that the vehicle is on, this parameter can be used to specify that lane. By default, this variable will be set to None, which means that the vehicle will follow the lane that it is currently on. + :param laneToFollow: If the lane to follow is different from the lane that the vehicle is on, this + parameter can be used to specify that lane. By default, this variable will be set to None, which + means that the vehicle will follow the lane that it is currently on. """ - past_steer_angle = 0 - past_speed = 0 # making an assumption here that the agent starts from zero speed - if laneToFollow is None: - current_lane = self.lane - else: - current_lane = laneToFollow - - current_centerline = current_lane.centerline - in_turning_lane = False # assumption that the agent is not instantiated within a connecting lane - intersection_passed = False - entering_intersection = False # assumption that the agent is not instantiated within an intersection - end_lane = None - original_target_speed = target_speed - TARGET_SPEED_FOR_TURNING = 5 # KM/H - TRIGGER_DISTANCE_TO_SLOWDOWN = 10 # FOR TURNING AT INTERSECTIONS - - if current_lane.maneuvers != (): - nearby_intersection = current_lane.maneuvers[0].intersection - if nearby_intersection == None: - nearby_intersection = current_lane.centerline[-1] - else: - nearby_intersection = current_lane.centerline[-1] + assert self.longitudinalController is not None + assert self.lateralController is not None - # instantiate longitudinal and lateral controllers - _lon_controller, _lat_controller = simulation().getLaneFollowingControllers(self) + path_metadata = None while True: + replan_time = 10 + min_path_distance = max(2*replan_time*target_speed, 50) + path, path_metadata = getFollowLanePath(self, min_path_distance, + preferStraight=preferStraight, laneToFollow=laneToFollow, path_metadata=path_metadata) + traj = Trajectory.createFixedSpeedTrajectory(path, target_speed, ts=simulation().timestep) + do FollowTrajectoryBehavior(traj) for replan_time seconds - if self.speed is not None: - current_speed = self.speed - else: - current_speed = past_speed +class Trajectory(object): + def __init__(self, polyline, ts): + assert isinstance(polyline, PolylineRegion) - if not entering_intersection and (distance from self.position to nearby_intersection) < TRIGGER_DISTANCE_TO_SLOWDOWN: - entering_intersection = True - intersection_passed = False - straight_manuevers = filter(lambda i: i.type == ManeuverType.STRAIGHT, current_lane.maneuvers) + self.polyline = polyline + self.ts = ts - if len(straight_manuevers) > 0: - select_maneuver = Uniform(*straight_manuevers) - else: - if len(current_lane.maneuvers) > 0: - select_maneuver = Uniform(*current_lane.maneuvers) - else: - take SetBrakeAction(1.0) - break + @property + def start(self): + return self.polyline.start - # assumption: there always will be a maneuver - if select_maneuver.connectingLane != None: - current_centerline = concatenateCenterlines([current_centerline, select_maneuver.connectingLane.centerline, \ - select_maneuver.endLane.centerline]) - else: - current_centerline = concatenateCenterlines([current_centerline, select_maneuver.endLane.centerline]) + @property + def end(self): + return self.polyline.end - current_lane = select_maneuver.endLane - end_lane = current_lane + @property + def duration(self): + return self.ts * len(self.polyline.points) - if current_lane.maneuvers != (): - nearby_intersection = current_lane.maneuvers[0].intersection - if nearby_intersection == None: - nearby_intersection = current_lane.centerline[-1] - else: - nearby_intersection = current_lane.centerline[-1] + @property + def length(self): + return self.polyline.length - if select_maneuver.type != ManeuverType.STRAIGHT: - in_turning_lane = True - target_speed = TARGET_SPEED_FOR_TURNING + def getRelativeTime(self, pos): + return toShapely(self.polyline).project(ShapelyPoint(*pos), normalized=True)*self.duration - do TurnBehavior(trajectory = current_centerline) + def getTimedDistance(self, timeA, timeB): + return shapely.ops.substring(toShapely(self.polyline), timeA/self.duration, timeB/self.duration, normalized=True).length + def __getitem__(self, time): + pt = toShapely(self.polyline).interpolate(time/self.duration, normalized=True) + return Vector(pt.x, pt.y) - if (end_lane is not None) and (self.position in end_lane) and not intersection_passed: - intersection_passed = True - in_turning_lane = False - entering_intersection = False - target_speed = original_target_speed - _lon_controller, _lat_controller = simulation().getLaneFollowingControllers(self) + @staticmethod + def createFixedSpeedTrajectory(polyline, targetSpeed, ts): + target_dist = 0 + points = [] + while target_dist < polyline.length: + points.append(polyline.lineString.interpolate(target_dist)) + target_dist += targetSpeed*ts - nearest_line_points = current_centerline.nearestSegmentTo(self.position) - nearest_line_segment = PolylineRegion(nearest_line_points) - cte = nearest_line_segment.signedDistanceTo(self.position) - if is_oppositeTraffic: - cte = -cte + return Trajectory(PolylineRegion(polyline=LineString(points)), ts=ts) - speed_error = target_speed - current_speed +behavior FollowTrajectoryBehavior(trajectory, terminationDistance=1): + """ + Follows the given Trajectory. + + The behavior terminates when either of the following conditions are met the vehicle position is within + terminationDistance of the end of the trajectory. - # compute throttle : Longitudinal Control - throttle = _lon_controller.run_step(speed_error) + Args: + trajectory: A Trajectory. + terminationDistance: The behavior will terminate when the vehicle position is within terminationDistance + of the end of the trajectory. + """ + assert isinstance(trajectory, Trajectory) + assert self.longitudinalController is not None + assert self.lateralController is not None + + while distance from self.position to trajectory.end > terminationDistance: + # Compute throttle : Longitudinal Control + throttle = self.longitudinalController.computeThrottle(trajectory, self) + if throttle > 0: + throttle_action = SetThrottleAction(throttle) + else: + throttle_action = SetBrakeAction(-throttle) - # compute steering : Lateral Control - current_steer_angle = _lat_controller.run_step(cte) + # Compute steering : Lateral Control + steer = self.lateralController.computeSteering(trajectory, self) + steer_action = SetSteerAction(steer) - take RegulatedControlAction(throttle, current_steer_angle, past_steer_angle) - past_steer_angle = current_steer_angle - past_speed = current_speed + take throttle_action, steer_action +## Legacy Behaviors ## -behavior FollowTrajectoryBehavior(target_speed = 10, trajectory = None, turn_speed=None): +def concatenateCenterlines(centerlines=[]): + return PolylineRegion.unionAll(centerlines) + +behavior FollowTrajectoryBehaviorOld(target_speed = 10, trajectory = None, turn_speed=None): """ Follows the given trajectory. The behavior terminates once the end of the trajectory is reached. @@ -204,9 +240,7 @@ behavior FollowTrajectoryBehavior(target_speed = 10, trajectory = None, turn_spe take RegulatedControlAction(throttle, current_steer_angle, past_steer_angle) past_steer_angle = current_steer_angle - - -behavior TurnBehavior(trajectory, target_speed=6): +behavior TurnBehavior(trajectory, target_speed=6, controllers=None): """ This behavior uses a controller specifically tuned for turning at an intersection. This behavior is only operational within an intersection, @@ -219,7 +253,10 @@ behavior TurnBehavior(trajectory, target_speed=6): trajectory_centerline = concatenateCenterlines([traj.centerline for traj in trajectory]) # instantiate longitudinal and lateral controllers - _lon_controller, _lat_controller = simulation().getTurningControllers(self) + if controllers: + _lon_controller, _lat_controller = controllers + else: + _lon_controller, _lat_controller = simulation().getTurningControllers(self) past_steer_angle = 0 diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic new file mode 100644 index 000000000..8031d5248 --- /dev/null +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -0,0 +1,292 @@ +import collections +import math +import shapely +from shapely.geometry import GeometryCollection, MultiPolygon, Polygon, MultiLineString, LineString, MultiPoint, Point as ShapelyPoint + +import scenic.domains.driving.model as _model +from scenic.core.regions import toShapely +from scenic.core.type_support import toVector +from scenic.domains.driving.actions import * + +## Pedestrian Behaviors +def getBugPath(actor, path_ls, backgroundObjects, bufferCalc, lookaheadTime, vehBuffer, nonVehBuffer): + """ Refine a walking path using a Bug algorithm approach.""" + assert isinstance(path_ls, LineString) + orig_path_ls = path_ls #TODO: TEMP + + # Lambda to compute buffer const. + baseBuffer = shapely.minimum_bounding_radius(actor._boundingPolygon) + bufferCalc = lambda obj: baseBuffer + (vehBuffer if obj.isVehicle else nonVehBuffer) + + # Compute the obstacle polygons, accounting for the plan of objects that have already logged it. + raw_obst_polys = [obj._boundingPolygon.buffer(bufferCalc(obj)) + for obj in backgroundObjects] + def future_poly_helper(obj): + planned_path, planned_speed = obj._planData + trimmed_path = shapely.ops.substring(planned_path, 0, planned_speed*lookaheadTime) + return trimmed_path.buffer(bufferCalc(obj) + + shapely.minimum_bounding_radius(obj._boundingPolygon)) + future_polys = [future_poly_helper(obj) for obj in backgroundObjects + if not obj.isVehicle and getattr(obj, "_planData", None) is not None] + + obst_multi_poly = shapely.union_all(raw_obst_polys + future_polys) + + if isinstance(obst_multi_poly, MultiPolygon): + obst_polys = obst_multi_poly.geoms + assert all(isinstance(geom, Polygon) for geom in obst_polys) + elif isinstance(obst_multi_poly, Polygon): + obst_polys = [obst_multi_poly] + elif obst_multi_poly.is_empty: + obst_polys = [] + else: + assert False + + # Refine path around obstacles, going from those with the largest boundary inwards + # (to account for the rare case where an obstacle poly may be entirely contained in another) + for obstacle_poly in sorted(obst_polys, key=lambda x: x.boundary.length, reverse=True): + self_pt = shapely.force_2d(ShapelyPoint(actor.position)) + target_pt = shapely.force_2d(ShapelyPoint(path_ls.coords[-1])) + + # TODO: Better handling so the pedestrian keeps walking towards goal inside a large poly. + if obstacle_poly.contains(target_pt): + # Check if target is inside the polygon. + # If we're too close to the exterior point, return None. + if obstacle_poly.distance(self_pt) < 0.01: + return None + + # Otherwise, truncate the path to the closest point on the exterior of the obstacle_poly. + exterior_intersection = path_ls.intersection(obstacle_poly.exterior) + stop_pt = shapely.ops.nearest_points(exterior_intersection, self_pt)[0] + path_ls = shapely.ops.substring(path_ls, 0, path_ls.project(stop_pt, normalized=True), normalized=True) + continue + + if path_ls.intersects(obstacle_poly): + # Find intersection points of path with exterior, and extract the first and + # last with respect to their distance along the path. + exterior_intersection = path_ls.intersection(obstacle_poly.exterior) + intersection_points = [] + + # If we're inside the obstacle poly, add the closest exterior point to guide us out. + if obstacle_poly.contains(self_pt): + intersection_points.append(path_ls.interpolate(path_ls.project(self_pt))) + + if isinstance(exterior_intersection, ShapelyPoint): + # If we are in the obstacle poly, add the intersection point to help guide us out. + # Otherwise, our destination is on the border of the poly itself, and we simply should continue. + if obstacle_poly.contains(self_pt): + + intersection_points.append(exterior_intersection) + else: + continue + elif isinstance(exterior_intersection, LineString): + intersection_points += [ + ShapelyPoint(exterior_intersection.coords[0]), + ShapelyPoint(exterior_intersection.coords[1]) + ] + elif isinstance(exterior_intersection, (MultiPoint, MultiLineString, GeometryCollection)): + for geom in exterior_intersection.geoms: + if isinstance(geom, ShapelyPoint): + intersection_points.append(geom) + elif isinstance(geom, LineString): + intersection_points += [ShapelyPoint(geom.coords[0]), ShapelyPoint(geom.coords[1])] + else: + assert False + + intersection_points.sort(key=lambda x: path_ls.project(x)) + start_pt = intersection_points[0] + end_pt = intersection_points[-1] + + # Split the exterior ring into two segments at these points + exterior_ls = LineString(obstacle_poly.exterior) + start_pt_s = exterior_ls.project(start_pt, normalized=True) + exterior_ls = LineString(list(shapely.ops.substring(exterior_ls, start_pt_s, 1, normalized=True).coords) + + list(shapely.ops.substring(exterior_ls, 0, start_pt_s, normalized=True).coords)) + exterior_ls = shapely.remove_repeated_points(exterior_ls) + end_pt_ls = exterior_ls.project(end_pt, normalized=True) + exterior_segments = [ + shapely.ops.substring(exterior_ls, 0, end_pt_ls, normalized=True), + shapely.ops.substring(exterior_ls, end_pt_ls, 1, normalized=True) + ] + + # Patch together the shorter of these exterior segments with the path. + start_path = shapely.ops.substring(path_ls, 0, path_ls.project(start_pt, normalized=True), normalized=True) + end_path = shapely.ops.substring(path_ls, path_ls.project(end_pt, normalized=True), 1, normalized=True) + start_path = shapely.force_2d(start_path) + end_path = shapely.force_2d(end_path) + + # Extract the mid_path. If paths are very close in length, + # bias to the right. + # TODO: Bias to the appropriate driving direction + if 0.95 < exterior_segments[0].length/exterior_segments[1].length < 1.05: + def angle_helper(ls): + return actor.apparentHeadingTo(Vector(*ls.centroid.coords[0])) + exterior_segments.sort(key=lambda x: angle_helper(x)) + else: + exterior_segments.sort(key=lambda x: x.length) + mid_path = exterior_segments[0] + if isinstance(mid_path, LineString): + mid_path = shapely.force_2d(mid_path) + + # Reverse the mid path if needed. + if (ShapelyPoint(mid_path.coords[0]).distance(start_pt) > ShapelyPoint(mid_path.coords[0]).distance(end_pt)): + mid_path = mid_path.reverse() + + # If the closest point on the mid path is very close, cut start path short + # and aim directly for it. This helps avoid backtracking loop. + if self_pt.distance(mid_path) < 1: + mid_path = shapely.ops.substring(mid_path, mid_path.project(self_pt, normalized=True), 1, normalized=True) + start_pt = ShapelyPoint(mid_path.coords[0]) + start_path = LineString([self_pt, start_pt]) + elif isinstance(mid_path, ShapelyPoint): + continue + else: + assert False, mid_path + + path_ls = LineString(list(start_path.coords) + list(mid_path.coords) + list(end_path.coords)) + path_ls = shapely.remove_repeated_points(path_ls) + + # from scenic.syntax.veneer import simulation + # if path_ls.intersects(obst_multi_poly.buffer(-0.5)):#simulation().currentRealTime > 8: + # import matplotlib.pyplot as plt + # plt.gca().set_aspect("equal") + # from scenic.core.geometry import plotPolygon + # from scenic.syntax.veneer import simulation + # simulation().scene.workspace.network.show() + # for obj in simulation().objects: + # obj.show2D(simulation().scene.workspace, plt) + # simulation().scene.workspace.zoomAround(plt, simulation().objects) + # plotPolygon(obst_multi_poly, plt, style="c--") + # plotPolygon(orig_path_ls, plt, style="y-") + # plotPolygon(path_ls, plt, style="g--") + # plt.show(block=False) + # breakpoint() + + return path_ls + +behavior TieBreakingPause(): + take SetWalkingSpeedAction(0) + wait for Range(0, 0.5) seconds + +behavior _WalkPathHelper(path, targetSpeed): + dist_along = 0 + while True: + # Determine target point, which will move along the path until we re-plan. + dist_along += targetSpeed*simulation().timestep + target_pt = Vector(*path.interpolate(dist_along).coords[0]) + # Set appropriate heading and velocity, calculating actual speed we should aim + # for, so we don't overshoot if we cut a corner or are at the end of the path. + actual_speed = min(targetSpeed, (distance from self to target_pt)/simulation().timestep) + heading = angle from self to target_pt + take SetWalkingDirectionAction(heading), SetWalkingSpeedAction(actual_speed) + +behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, + terminationThresh=0.1, replanTime=0.1, lookaheadTime=4, + vehBuffer=2, nonVehBuffer=0.2): + """ Walk a path at targetSpeed, stopping at the end.""" + if not isinstance(path, PolylineRegion): + raise ValueError("`path` must be a `PolylineRegion`.") + + bufferCalc = lambda obj: (shapely.minimum_bounding_radius(self._boundingPolygon) + + (vehBuffer if obj.isVehicle else nonVehBuffer)) + + while distance from self to path.end > terminationThresh: + path_ls = shapely.force_2d(path.lineString) + self_pt = (self.position.x, self.position.y) + + # Refine the path by dropping already traversed areas and adding a link to the start. + start_s = path_ls.project(ShapelyPoint(self_pt), normalized=True) + path_ls = shapely.ops.substring(path_ls, start_s, 1, normalized=True) + path_ls = LineString([self_pt] + list(path_ls.coords)) + path_ls = shapely.remove_repeated_points(path_ls) + + # If our immediate path has us cross through any objects in motion, stop and + # wait until it's clear. + background_objects = [obj for obj in simulation().objects if obj is not self] + immediate_path = shapely.ops.substring(path_ls, 0, targetSpeed) + danger_objects = [obj for obj in background_objects if obj.speed > 0.44 and obj.isVehicle] + moving_obj_danger_zone = shapely.union_all( + [obj._boundingPolygon.buffer(bufferCalc(obj)) for obj in danger_objects] + ) + if immediate_path.intersects(moving_obj_danger_zone): + do TieBreakingPause() + continue + + # Modify path to route around objects. + path_ls = getBugPath(self, path_ls, background_objects, bufferCalc, lookaheadTime, vehBuffer, nonVehBuffer) + self._planData = path_ls, targetSpeed + + # If path_ls is None, our goal is inside the danger zone and we can't + # proceed further right now. + if path_ls is None: + do TieBreakingPause() + continue + + # Follow the path until we replan, terminating early if we reach the end. + try: + do _WalkPathHelper(path_ls, targetSpeed) for replanTime seconds + interrupt when distance from self to Vector(*path_ls.coords[-1]) < terminationThresh: + abort + + self._planData = None + + take SetWalkingSpeedAction(0) + +behavior WalkTo(target, targetSpeed, *, avoidObstacles=True): + """ Walk towards a given target position at targetSpeed, stopping at the end.""" + path = PolylineRegion(points=(self.position, toVector(target))) + do WalkPath(path, targetSpeed, avoidObstacles=avoidObstacles) + +behavior Walk(targetSpeed=None, backwards=None, avoidObstacles=True): + if targetSpeed is None: + # TODO: Should we move this to a property of pedestrians? (`baseWalkSpeed`?) + targetSpeed = Range(0.9, 1.8) # From ~2mph to ~4mph + + if backwards is None: + backwards = Uniform(True, False) + + network = _model.network + + # TODO: Have pedestrians bias towards the appropriate side of the sidewalk based off road direction? + while True: + # If we're not currently in a walkable region, return to the closest one. + if self.position not in network.walkableRegion: + # TODO: Replace with closest point in region operator. + closest_pt = shapely.ops.nearest_points(toShapely(network.walkableRegion), toShapely(self.position))[0] + target_element = network.findPointIn(Vector(*closest_pt.coords[0]), network.sidewalks+network.crossings, reject=False) + target_pt = target_element.centerline.project(self.position) + do WalkTo(target_pt, targetSpeed=targetSpeed, avoidObstacles=avoidObstacles) + continue + + # If we're not close to the start or end of the centerline of our current element + # (depending on whether we are walking `backwards` or not), walk towards it following the centerline. + current_element = network.findPointIn(self.position, network.sidewalks+network.crossings, reject=False) + end_pt = current_element.centerline.start if backwards else current_element.centerline.end + if distance from self.position to end_pt > 0.1: + target_path = current_element.centerline.reverse() if backwards else current_element.centerline + do WalkPath(target_path, targetSpeed=targetSpeed, avoidObstacles=avoidObstacles) + continue + + # If we're at the end of the current element, we should pick a successor/predecessor + # (depending on whether we are walking `backwards`). + # TODO: Randomly pick from ALL successors/predecessors and sidewalks. + next_element = current_element._predecessor if backwards else current_element._successor + if next_element is not None: + target_path = next_element.centerline.reverse() if backwards else next_element.centerline + do WalkPath(target_path, targetSpeed=targetSpeed, avoidObstacles=avoidObstacles) + continue + + # We have no valid next moves. Terminate the behavior. + # TODO: Turn around instead? + return + +# TODO: This uses WalkTowardsAction which doesn't exist. +behavior WalkForwardBehavior(): + """Walk forward behavior for pedestrians. + + It will uniformly randomly choose either end of the sidewalk that + the pedestrian is on, and have the pedestrian walk towards the endpoint. + """ + current_sidewalk = _model.network.sidewalkAt(self.position) + end_point = Uniform(*current_sidewalk.centerline.points) + end_vec = end_point[0] @ end_point[1] + normal_vec = Vector.normalized(end_vec) + take WalkTowardsAction(goal_position=normal_vec), SetSpeedAction(speed=1) diff --git a/src/scenic/domains/driving/controllers.py b/src/scenic/domains/driving/controllers.py index ce7d46135..f45298186 100644 --- a/src/scenic/domains/driving/controllers.py +++ b/src/scenic/domains/driving/controllers.py @@ -12,98 +12,157 @@ .. _CARLA: https://carla.org/ """ +from abc import ABC, abstractmethod from collections import deque +import math import numpy as np +import shapely +from shapely.geometry import LineString, MultiPoint, Point as ShapelyPoint +from scenic.core.regions import CircularRegion, PolylineRegion, toPolygon +from scenic.core.vectors import Vector -class PIDLongitudinalController: + +class LongitudinalController(ABC): + @abstractmethod + def computeThrottle(self, trajectory, veh): + pass + + +class LateralController(ABC): + @abstractmethod + def computeSteering(self, trajectory, veh): + pass + + +class PIDController: + def __init__(self, dt, *, K_P, K_D, K_I, wg): + super().__init__() + self.dt = dt + self.kp = K_P + self.ki = K_I + self.kd = K_D + self.i_term = 0 + self.last_error = None + self.windup_guard = 0.5 / self.ki if self.ki != 0 else 0 + + def run_step(self, error): + # Compute terms + p_term = error + self.i_term += error * self.dt + self.i_term = np.clip(self.i_term, -self.windup_guard, self.windup_guard) + d_term = (error - self.last_error) / self.dt if self.last_error else 0 + + # Remember last error for next calculation + self.last_error = error + + output = (self.kp * p_term) + (self.ki * self.i_term) + (self.kd * d_term) + clipped_output = np.clip(output, -1, 1) + return clipped_output + + +class PIDLongitudinalController(PIDController, LongitudinalController): """Longitudinal control using a PID to reach a target speed. Arguments: + dt: time step K_P: Proportional gain K_D: Derivative gain K_I: Integral gain - dt: time step + wg: The windup guard's cap on the integral components contribution + to the total control signal. """ - def __init__(self, K_P=0.5, K_D=0.1, K_I=0.2, dt=0.1): - self._k_p = K_P - self._k_d = K_D - self._k_i = K_I - self._dt = dt - self._error_buffer = deque(maxlen=10) + def __init__(self, dt=0.1, *, K_P=0.5, K_D=0.1, K_I=0.2, wg=0.5): + super().__init__(dt=dt, K_P=K_P, K_D=K_D, K_I=K_I, wg=wg) - def run_step(self, speed_error): - """Estimate the throttle/brake of the vehicle based on the PID equations. + def computeThrottle(self, trajectory, veh): + curr_time = trajectory.getRelativeTime(veh.position) + ts_dist = trajectory.getTimedDistance(curr_time, curr_time + trajectory.ts) + target_speed = ts_dist / trajectory.ts - Arguments: - speed_error: target speed minus current speed + cte = target_speed - veh.speed + return self.run_step(cte) - Returns: - a signal between -1 and 1, with negative values indicating braking. - """ - error = speed_error - self._error_buffer.append(error) - if len(self._error_buffer) >= 2: - _de = (self._error_buffer[-1] - self._error_buffer[-2]) / self._dt - _ie = sum(self._error_buffer) * self._dt - else: - _de = 0.0 - _ie = 0.0 - - return np.clip( - (self._k_p * error) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0 - ) - - -class PIDLateralController: +class PIDLateralController(PIDController, LateralController): """Lateral control using a PID to track a trajectory. Arguments: + dt: time step K_P: Proportional gain K_D: Derivative gain K_I: Integral gain - dt: time step + wg: The windup guard's cap on the integral components contribution + to the total control signal. """ - def __init__(self, K_P=0.3, K_D=0.2, K_I=0, dt=0.1): - self.Kp = K_P - self.Kd = K_D - self.Ki = K_I - self.PTerm = 0 - self.ITerm = 0 - self.DTerm = 0 - self.dt = dt - self.last_error = 0 - self.windup_guard = 20.0 - self.output = 0 - - def run_step(self, cte): - """Estimate the steering angle of the vehicle based on the PID equations. + def __init__(self, dt=0.1, *, K_P=0.3, K_D=0.2, K_I=0, wg=0): + super().__init__(dt=dt, K_P=K_P, K_D=K_D, K_I=K_I, wg=wg) - Arguments: - cte: cross-track error (distance to right of desired trajectory) + def computeSteering(self, trajectory, veh): + cte = trajectory.polyline.signedDistanceTo(veh.position) + steer_angle = self.run_step(cte) + return steer_angle - Returns: - a signal between -1 and 1, with -1 meaning maximum steering to the left. - """ - error = cte - delta_error = error - self.last_error - self.PTerm = self.Kp * error - self.ITerm += error * self.dt - if self.ITerm < -self.windup_guard: - self.ITerm = -self.windup_guard - elif self.ITerm > self.windup_guard: - self.ITerm = self.windup_guard +class PurePursuitLateralController(LateralController): + def __init__(self, lookaheadDistance=lambda veh: veh.speed + 1): + super().__init__() + self.lookaheadDistance = lookaheadDistance + self._lastTargetPoint = None - self.DTerm = delta_error / self.dt + def _findTargetPoint(self, trajectory, veh, lookaheadDistance): + traj_line_string = toPolygon(trajectory.polyline) - # Remember last error for next calculation - self.last_error = error + # Find candidate target points + veh_pt = ShapelyPoint(*veh.position) + veh_traj_dist = traj_line_string.project(veh_pt) + forward_traj = shapely.ops.substring( + traj_line_string, veh_traj_dist, traj_line_string.length + ) + lookahead_circle = toPolygon( + CircularRegion(forward_traj.coords[0], lookaheadDistance) + ) + intersection_geometry = lookahead_circle.boundary.intersection(forward_traj) + + if intersection_geometry.is_empty: + # No viable target points. If we have a last target point, aim for that. Otherwise, + # aim for the closest point on the trajectory. + if self._lastTargetPoint: + target_point = self._lastTargetPoint + else: + target_point = trajectory.polyline.project(veh.position) + + # Our target point isn't actually at the correct lookaheadDistance, so we need to update it. + lookaheadDistance = shapely.distance(veh_pt, target_point) + elif isinstance(intersection_geometry, ShapelyPoint): + target_point = intersection_geometry + elif isinstance(intersection_geometry, MultiPoint): + # There are multiple candidate target points. Pick the one that appears first on the path. + target_point = sorted( + intersection_geometry.geoms, key=lambda pt: traj_line_string.project(pt) + )[0] + else: + # We've gotten something strange. Fall back to a representative point as the target. + target_point = intersection_geometry.representative_point() + + # Store last target point and return + self._lastTargetPoint = target_point + return Vector(target_point.x, target_point.y) + + def computeSteering(self, trajectory, veh): + # Compute target steering angle + lookaheadDistance = self.lookaheadDistance(veh) + targetPoint = self._findTargetPoint(trajectory, veh, lookaheadDistance) + rw_position = veh.position.offsetRotated( + veh.heading, Vector(0, -0.5 * veh.wheelbase, 0) + ) + alpha = rw_position.angleTo(targetPoint) - veh.heading + delta = -math.atan2(2 * veh.wheelbase * math.sin(alpha), lookaheadDistance) - self.output = self.PTerm + (self.Ki * self.ITerm) + (self.Kd * self.DTerm) + # Convert target steering angle to relative value in [-1, 1] + rel_steering_angle = np.clip(delta / veh.maxSteeringAngle, -1, 1) - return np.clip(self.output, -1, 1) + return rel_steering_angle diff --git a/src/scenic/domains/driving/model.scenic b/src/scenic/domains/driving/model.scenic index 40191b14b..3526a8990 100644 --- a/src/scenic/domains/driving/model.scenic +++ b/src/scenic/domains/driving/model.scenic @@ -134,6 +134,10 @@ class DrivingObject: def isVehicle(self): return False + @property + def isPedestrian(self): + return False + @property def isCar(self): return False @@ -266,6 +270,9 @@ class DrivingObject: def setVelocity(self, vel): raise NotImplementedError + def setOrientation(self, orientation): + raise NotImplementedError + class Vehicle(DrivingObject): """Vehicles which drive, such as cars. @@ -282,6 +289,24 @@ class Vehicle(DrivingObject): color (:obj:`Color` or RGB tuple): Color of the vehicle. The default value is a distribution derived from car color popularity statistics; see :obj:`Color.defaultCarColor`. + wheelbase: The distance between the front and rear axles of the vehicle. Default value is 0.6 + times the length of the vehicle. + maxSteeringAngle: The maximum steering angle of the vehicle. The full steering range would be + two times this value, going from (-maxSteeringAngle, maxSteeringAngle). Default value + 40 degrees. + wheelDiameter: The diameter of the *entire* wheel (including the tire). Default value is 0.7 meters. + trackWidth: Distance between the vehicle's wheels when pointed straight ahead. Default value + is 0.85 times the width of the vehicle. + groundClearance: Default value is half the wheel diameter. + maxSpeed: The maximum rated speed of the vehicle. Default value is 45 meters per second (~100 mph). + This value is not enforced by Scenic and is provided simply for other tools to reference (e.g. + exporting to OpenScenarioXML). + maxAcceleration: The maximum rated acceleration of the vehicle. Default value is 5 meters per second^2. + This value is not enforced by Scenic and is provided simply for other tools to reference (e.g. + exporting to OpenScenarioXML). + maxDeceleration: The maximum rated deceleration of the vehicle. Default value is 10 meters per second^2. + This value is not enforced by Scenic and is provided simply for other tools to reference (e.g. + exporting to OpenScenarioXML). """ regionContainedIn: roadOrShoulder position: new Point on road @@ -292,10 +317,30 @@ class Vehicle(DrivingObject): length: 4.5 color: Color.defaultCarColor() + lateralController: None + longitudinalController: None + + wheelbase: 0.6*self.length + maxSteeringAngle: 40 deg + wheelDiameter: 0.7 + trackWidth: 0.85*self.width + groundClearance: 0.5*self.wheelDiameter + maxSpeed: 45 + maxAcceleration: 5 + maxDeceleration: 10 + @property def isVehicle(self): return True + def startDynamicSimulation(self): + defaultLongitudinalController, defaultLateralController = simulation().getLaneFollowingControllers(self) + + if self.longitudinalController is None: + self.longitudinalController = defaultLongitudinalController + if self.lateralController is None: + self.lateralController = defaultLateralController + class Car(Vehicle): """A car.""" @property @@ -317,6 +362,7 @@ class Pedestrian(DrivingObject): length: The default length is 0.75 m. color: The default color is turquoise. Pedestrian colors are not necessarily used by simulators, but do appear in the debugging diagram. + mass: Default value is 65 kg. """ regionContainedIn: network.walkableRegion position: new Point on network.walkableRegion @@ -325,6 +371,11 @@ class Pedestrian(DrivingObject): width: 0.75 length: 0.75 color: [0, 0.5, 1] + mass: 65 + + @property + def isPedestrian(self): + return True ## Stub sensor implementations diff --git a/src/scenic/domains/driving/roads.py b/src/scenic/domains/driving/roads.py index f06377c5a..4c72ad7bd 100644 --- a/src/scenic/domains/driving/roads.py +++ b/src/scenic/domains/driving/roads.py @@ -888,6 +888,7 @@ class Network: sidewalkRegion: PolygonalRegion = None curbRegion: PolylineRegion = None shoulderRegion: PolygonalRegion = None + centerlineRegion: PolylineRegion = None #: Traffic flow vector field aggregated over all roads (0 elsewhere). roadDirection: VectorField = None @@ -950,6 +951,11 @@ def __attrs_post_init__(self): edges.append(road.backwardLanes.curb) self.curbRegion = PolylineRegion.unionAll(edges) + if self.centerlineRegion is None: + self.centerlineRegion = PolylineRegion.unionAll( + [lane.centerline for lane in self.lanes] + ) + if self.roadDirection is None: # TODO replace with a PolygonalVectorField for better pruning self.roadDirection = VectorField("roadDirection", self._defaultRoadDirection) @@ -987,7 +993,7 @@ def _currentFormatVersion(cls): :meta private: """ - return 35 + return 36 class DigestMismatchError(Exception): """Exception raised when loading a cached map not matching the original file.""" diff --git a/src/scenic/formats/opendrive/xodr_parser.py b/src/scenic/formats/opendrive/xodr_parser.py index 8175d2c83..cf274db09 100644 --- a/src/scenic/formats/opendrive/xodr_parser.py +++ b/src/scenic/formats/opendrive/xodr_parser.py @@ -1143,7 +1143,7 @@ def getEdges(forward): leftEdge=leftEdge, rightEdge=rightEdge, road=None, - lanes=tuple(backwardLanes), + lanes=tuple(reversed(backwardLanes)), curb=(backwardShoulder.rightEdge if backwardShoulder else rightEdge), sidewalk=backwardSidewalk, bikeLane=None, diff --git a/src/scenic/simulators/metadrive/model.scenic b/src/scenic/simulators/metadrive/model.scenic index 078c6fc33..1d3308884 100644 --- a/src/scenic/simulators/metadrive/model.scenic +++ b/src/scenic/simulators/metadrive/model.scenic @@ -125,6 +125,9 @@ class MetaDriveActor(DrivingObject): def setVelocity(self, vel): self.metaDriveActor.set_velocity(vel) + def setOrientation(self, orientation): + converted_heading = scenicToMetaDriveHeading(orientation.yaw) + self.metaDriveActor.set_heading_theta(converted_heading) class Vehicle(Vehicle, Steers, MetaDriveActor): def __init__(self, *args, **kwargs): @@ -184,7 +187,7 @@ class Pedestrian(Pedestrian, MetaDriveActor, Walks): return True def setWalkingDirection(self, heading): - self._walking_direction = scenicToMetaDriveHeading(heading) + self._walking_direction = heading def setWalkingSpeed(self, speed): self._walking_speed = speed diff --git a/src/scenic/simulators/metadrive/simulator.py b/src/scenic/simulators/metadrive/simulator.py index 76607d5bc..abba972ec 100644 --- a/src/scenic/simulators/metadrive/simulator.py +++ b/src/scenic/simulators/metadrive/simulator.py @@ -39,7 +39,7 @@ def __init__( timestep=0.1, render=True, render3D=False, - real_time=True, + real_time=None, screen_record=False, screen_record_filename=None, screen_record_path="metadrive_gifs", @@ -51,7 +51,10 @@ def __init__( self.timestep = timestep self.sumo_map = sumo_map self.xodr_map = xodr_map - self.real_time = real_time + if real_time is None: + self.real_time = self.render or self.render3D + else: + self.real_time = real_time self.screen_record = screen_record self.screen_record_filename = screen_record_filename self.screen_record_path = screen_record_path @@ -283,13 +286,15 @@ def executeActions(self, allActions): else: # For Pedestrians if obj._walking_direction is None: - obj._walking_direction = utils.scenicToMetaDriveHeading(obj.heading) + obj._walking_direction = obj.heading if obj._walking_speed is None: obj._walking_speed = obj.speed + metadrive_heading = utils.scenicToMetaDriveHeading(obj._walking_direction) direction = [ - math.cos(obj._walking_direction), - math.sin(obj._walking_direction), + math.cos(metadrive_heading), + math.sin(metadrive_heading), ] + obj.setOrientation(toOrientation(obj._walking_direction)) obj.metaDriveActor.set_velocity(direction, obj._walking_speed) def step(self): diff --git a/src/scenic/simulators/newtonian/simulator.py b/src/scenic/simulators/newtonian/simulator.py index 4c23adaec..61876e99b 100644 --- a/src/scenic/simulators/newtonian/simulator.py +++ b/src/scenic/simulators/newtonian/simulator.py @@ -214,7 +214,9 @@ def step(self): if obj.control["speed"] is not None else obj.speed ) + obj.heading = h obj.velocity = Vector(0, s).rotatedBy(h) + obj.speed = obj.velocity.norm() # 2) Vehicle: throttle/brake/steer physics elif getattr(obj, "isCar", False): forward = obj.velocity.dot(Vector(0, 1).rotatedBy(obj.heading)) >= 0 diff --git a/src/scenic/simulators/utils/colors.py b/src/scenic/simulators/utils/colors.py index 7f3a0bed9..1bfe8b747 100644 --- a/src/scenic/simulators/utils/colors.py +++ b/src/scenic/simulators/utils/colors.py @@ -78,6 +78,8 @@ class NoisyColorDistribution(Distribution): lightNoise (float): noise to add to base lightness """ + _deterministic = True + def __init__(self, baseColor, hueNoise, satNoise, lightNoise): super().__init__(baseColor, hueNoise, satNoise, lightNoise, valueType=Color) self.baseColor = baseColor diff --git a/src/scenic/syntax/__init__.py b/src/scenic/syntax/__init__.py index 9ea3b509d..66d65ce1c 100644 --- a/src/scenic/syntax/__init__.py +++ b/src/scenic/syntax/__init__.py @@ -1,5 +1,6 @@ """The Scenic compiler and associated support code.""" +import hashlib as _hashlib import pathlib as _pathlib import subprocess as _subprocess import sys as _sys @@ -8,6 +9,7 @@ _projectRootDir = _syntaxDir.parent.parent.parent _grammarPath = _syntaxDir / "scenic.gram" _parserPath = _syntaxDir / "parser.py" +_checksumPath = _syntaxDir / ".parser_checksum" def buildParser(): @@ -30,10 +32,30 @@ def buildParser(): capture_output=True, text=True, ) + + with open(_checksumPath, "wb") as f: + f.write(getGrammarHash()) + return result -if not _parserPath.exists(): +def getGrammarHash(): + with open(_grammarPath, "rb") as f: + data = f.read() + return _hashlib.blake2b(data).digest() + + +def checksumValid(): + if not _checksumPath.exists(): + return False + + with open(_checksumPath, "rb") as f: + checksum = f.read() + + return checksum == getGrammarHash() + + +if not _parserPath.exists() or not checksumValid(): _result = buildParser() _retcode = _result.returncode if _retcode != 0: diff --git a/src/scenic/syntax/ast.py b/src/scenic/syntax/ast.py index f21a49aaf..3dfc4c20a 100644 --- a/src/scenic/syntax/ast.py +++ b/src/scenic/syntax/ast.py @@ -435,7 +435,12 @@ class RelativeHeadingOp(AST): base: Optional[ast.AST] = None -class ApparentHeadingOp(AST): +class ApparentHeadingOfOp(AST): + target: ast.AST + base: Optional[ast.AST] = None + + +class ApparentHeadingToOp(AST): target: ast.AST base: Optional[ast.AST] = None @@ -446,6 +451,12 @@ class DistanceFromOp(AST): base: Optional[ast.AST] = None +class MinDistanceFromOp(AST): + # because `to` and `from` are symmetric, the first operand will be `target` and the second will be `base` + target: ast.AST + base: Optional[ast.AST] = None + + class DistancePastOp(AST): target: ast.AST base: Optional[ast.AST] = None @@ -650,3 +661,23 @@ class CanSeeOp(AST): class IntersectsOp(AST): left: ast.AST right: ast.AST + + +class AheadOfOp(AST): + left: ast.AST + right: ast.AST + + +class BehindOp(AST): + left: ast.AST + right: ast.AST + + +class LeftOfOp(AST): + left: ast.AST + right: ast.AST + + +class RightOfOp(AST): + left: ast.AST + right: ast.AST diff --git a/src/scenic/syntax/compiler.py b/src/scenic/syntax/compiler.py index 0bf027823..2dbad82a2 100644 --- a/src/scenic/syntax/compiler.py +++ b/src/scenic/syntax/compiler.py @@ -1249,9 +1249,9 @@ def visit_TerminateSimulation(self, node: s.TerminateSimulation): @context(Context.DYNAMIC) def visit_Do(self, node: s.Do): - if (self.inBehavior or self.inMonitor) and len(node.elts) > 1: + if self.inMonitor and len(node.elts) > 1: raise self.makeSyntaxError( - f"`do` can only take one action inside a {'behavior' if self.inBehavior else 'monitor'}", + f"`do` can only take one action inside a monitor", node, ) return self.makeDoLike(node, node.elts) @@ -1393,6 +1393,18 @@ def visit_TerminateSimulationWhen(self, node: s.TerminateSimulationWhen): "terminate_simulation_when", node.cond, node.lineno, node.name ) + @context(Context.TOP_LEVEL) + def visit_TerminateAfter(self, node: s.TerminateAfter): + cond = ast.Call( + func=ast.Name(id="check_time", ctx=loadCtx), + args=[ + self.visit(node.duration.value), + ast.Constant(node.duration.unitStr), + ], + keywords=[], + ) + return self.createRequirementLike("terminate_after", cond, node.lineno, None) + def createRequirementLike( self, functionName: str, @@ -1435,22 +1447,6 @@ def createRequirementLike( ) ) - @context(Context.TOP_LEVEL) - def visit_TerminateAfter(self, node: s.TerminateAfter): - return ast.copy_location( - ast.Expr( - ast.Call( - func=ast.Name(id="terminate_after", ctx=loadCtx), - args=[ - self.visit(node.duration.value), - ast.Constant(node.duration.unitStr), - ], - keywords=[], - ) - ), - node, - ) - @context(Context.TOP_LEVEL) def visit_Simulator(self, node: s.Simulator): return ast.copy_location( @@ -1683,9 +1679,20 @@ def visit_RelativeHeadingOp(self, node: s.RelativeHeadingOp): ), ) - def visit_ApparentHeadingOp(self, node: s.ApparentHeadingOp): + def visit_ApparentHeadingOfOp(self, node: s.ApparentHeadingOfOp): + return ast.Call( + func=ast.Name(id="ApparentHeadingOf", ctx=loadCtx), + args=[self.visit(node.target)], + keywords=( + [] + if node.base is None + else [ast.keyword(arg="Y", value=self.visit(node.base))] + ), + ) + + def visit_ApparentHeadingToOp(self, node: s.ApparentHeadingToOp): return ast.Call( - func=ast.Name(id="ApparentHeading", ctx=loadCtx), + func=ast.Name(id="ApparentHeadingTo", ctx=loadCtx), args=[self.visit(node.target)], keywords=( [] @@ -1705,6 +1712,17 @@ def visit_DistanceFromOp(self, node: s.DistanceFromOp): ), ) + def visit_MinDistanceFromOp(self, node: s.MinDistanceFromOp): + return ast.Call( + func=ast.Name(id="MinDistanceFrom", ctx=loadCtx), + args=[self.visit(node.target)], + keywords=( + [ast.keyword(arg="Y", value=self.visit(node.base))] + if node.base is not None + else [] + ), + ) + def visit_DistancePastOp(self, node: s.DistancePastOp): return ast.Call( func=ast.Name(id="DistancePast", ctx=loadCtx), @@ -1850,3 +1868,43 @@ def visit_IntersectsOp(self, node: s.IntersectsOp): ], keywords=[], ) + + def visit_AheadOfOp(self, node: s.AheadOfOp): + return ast.Call( + func=ast.Name(id="AheadOfOp", ctx=loadCtx), + args=[ + self.visit(node.left), + self.visit(node.right), + ], + keywords=[], + ) + + def visit_BehindOp(self, node: s.BehindOp): + return ast.Call( + func=ast.Name(id="BehindOp", ctx=loadCtx), + args=[ + self.visit(node.left), + self.visit(node.right), + ], + keywords=[], + ) + + def visit_LeftOfOp(self, node: s.LeftOfOp): + return ast.Call( + func=ast.Name(id="LeftOfOp", ctx=loadCtx), + args=[ + self.visit(node.left), + self.visit(node.right), + ], + keywords=[], + ) + + def visit_RightOfOp(self, node: s.RightOfOp): + return ast.Call( + func=ast.Name(id="RightOfOp", ctx=loadCtx), + args=[ + self.visit(node.left), + self.visit(node.right), + ], + keywords=[], + ) diff --git a/src/scenic/syntax/scenic.gram b/src/scenic/syntax/scenic.gram index ec3e63f10..188f84626 100644 --- a/src/scenic/syntax/scenic.gram +++ b/src/scenic/syntax/scenic.gram @@ -1803,6 +1803,10 @@ bitwise_or: | scenic_not_visible_from | scenic_can_see | scenic_intersects + | a=bitwise_or "ahead" "of" b=bitwise_xor { s.AheadOfOp(left=a, right=b, LOCATIONS) } + | a=bitwise_or "behind" b=bitwise_xor { s.BehindOp(left=a, right=b, LOCATIONS) } + | a=bitwise_or "left" "of" b=bitwise_xor { s.LeftOfOp(left=a, right=b, LOCATIONS) } + | a=bitwise_or "right" "of" b=bitwise_xor { s.RightOfOp(left=a, right=b, LOCATIONS) } | a=bitwise_or '|' b=bitwise_xor { ast.BinOp(left=a, op=ast.BitOr(), right=b, LOCATIONS) } | bitwise_xor @@ -1846,13 +1850,18 @@ scenic_prefix_operators: | "relative" "heading" "of" e1=expression 'from' e2=scenic_prefix_operators { s.RelativeHeadingOp(target=e1, base=e2, LOCATIONS) } | "relative" "heading" "of" e1=scenic_prefix_operators { s.RelativeHeadingOp(target=e1, LOCATIONS) } # apparent heading of - | "apparent" "heading" "of" e1=expression 'from' e2=scenic_prefix_operators { s.ApparentHeadingOp(target=e1, base=e2, LOCATIONS) } - | "apparent" "heading" "of" e1=scenic_prefix_operators { s.ApparentHeadingOp(target=e1, LOCATIONS) } + | "apparent" "heading" "of" e1=expression 'from' e2=scenic_prefix_operators { s.ApparentHeadingOfOp(target=e1, base=e2, LOCATIONS) } + | "apparent" "heading" "of" e1=scenic_prefix_operators { s.ApparentHeadingOfOp(target=e1, LOCATIONS) } + # apparent heading to + | "apparent" "heading" "to" e1=expression 'from' e2=scenic_prefix_operators { s.ApparentHeadingToOp(target=e1, base=e2, LOCATIONS) } + | "apparent" "heading" "to" e1=scenic_prefix_operators { s.ApparentHeadingToOp(target=e1, LOCATIONS) } # distance from/to | &"distance" scenic_distance_from_op # distance past | "distance" "past" e1=expression 'of' e2=scenic_prefix_operators { s.DistancePastOp(target=e1, base=e2, LOCATIONS) } | "distance" "past" e1=scenic_prefix_operators { s.DistancePastOp(target=e1, LOCATIONS) } + # minimum distance from/to + | &"minimum" scenic_min_distance_from_op # angle from/to | &"angle" scenic_angle_from_op # altitude from/to @@ -1868,6 +1877,11 @@ scenic_distance_from_op: | "distance" 'to' e1=expression 'from' e2=scenic_prefix_operators { s.DistanceFromOp(target=e1, base=e2, LOCATIONS) } | "distance" ('to'|'from') e1=scenic_prefix_operators { s.DistanceFromOp(target=e1, LOCATIONS) } +scenic_min_distance_from_op: + | "minimum" "distance" 'from' e1=expression 'to' e2=scenic_prefix_operators { s.MinDistanceFromOp(target=e1, base=e2, LOCATIONS) } + | "minimum" "distance" 'to' e1=expression 'from' e2=scenic_prefix_operators { s.MinDistanceFromOp(target=e1, base=e2, LOCATIONS) } + | "minimum" "distance" ('to'|'from') e1=scenic_prefix_operators { s.MinDistanceFromOp(target=e1, LOCATIONS) } + scenic_angle_from_op: | "angle" 'from' e1=expression 'to' e2=scenic_prefix_operators { s.AngleFromOp(base=e1, target=e2, LOCATIONS) } | "angle" 'to' e1=expression 'from' e2=scenic_prefix_operators { s.AngleFromOp(target=e1, base=e2, LOCATIONS) } diff --git a/src/scenic/syntax/translator.py b/src/scenic/syntax/translator.py index 58e856607..d733d2bd3 100644 --- a/src/scenic/syntax/translator.py +++ b/src/scenic/syntax/translator.py @@ -29,16 +29,19 @@ import importlib.util import inspect import io +import itertools import os import sys import time import types from typing import Optional +import warnings -from scenic.core.distributions import RejectionException, toDistribution +from scenic.core.distributions import Distribution, RejectionException, toDistribution from scenic.core.dynamics.scenarios import DynamicScenario import scenic.core.errors as errors from scenic.core.errors import InvalidScenarioError, PythonCompileError +from scenic.core.external_params import ExternalParameter, ExternalSampler from scenic.core.lazy_eval import needsLazyEvaluation import scenic.core.pruning as pruning from scenic.core.serialization import deterministicHash @@ -161,6 +164,17 @@ def _scenarioFromStream( behavior as importing a Python module. See `purgeModulesUnsafeToCache` for a more detailed discussion of the internals behind this. """ + # Backup stream and parameters + streamLines = stream.read() + scenarioCreationData = { + "streamLines": streamLines, + "compileOptions": compileOptions, + "filename": filename, + "scenario": scenario, + "path": path, + } + stream = io.BytesIO(streamLines) + # Compile the code as if it were a top-level module oldModules = list(sys.modules.keys()) try: @@ -170,7 +184,9 @@ def _scenarioFromStream( if not _cacheImports: purgeModulesUnsafeToCache(oldModules) # Construct a Scenario from the resulting namespace - return constructScenarioFrom(namespace, scenario) + scenario = constructScenarioFrom(namespace, scenario) + scenario._scenarioCreationData = scenarioCreationData + return scenario @contextmanager @@ -690,4 +706,40 @@ def isModularScenario(thing): # Validate scenario scenario.validate() + # Convert distributions to ExternalParameters if requested, and create + # the external sampler. + if scenario.params.get("convertDistributions", False): + from scenic.core.external_params.verifai import VerifaiSampler + + externalParamConverter = scenario.params.get( + "externalSampler", VerifaiSampler + ).getExternalParameterConverter() + + for dep in scenario.dependencies: + externalParamConverter.convert(dep) + print(f"{dep}: {len(set(externalParamConverter.externalParams))}") + + ## DEBUG ## + test = { + d + for dep in scenario.dependencies + for d in dep.recursiveDependencies() + if ( + isinstance(d, Distribution) + and not d._conditioned._deterministic + and not isinstance(d._conditioned, ExternalParameter) + ) + } + for d in test: + print() + print(type(d)) + print(d) + print() + + newExternalParams = externalParamConverter.externalParams + else: + newExternalParams = [] + + scenario.createExternalSampler(newExternalParams) + return scenario diff --git a/src/scenic/syntax/veneer.py b/src/scenic/syntax/veneer.py index b79745030..ed9b3affe 100644 --- a/src/scenic/syntax/veneer.py +++ b/src/scenic/syntax/veneer.py @@ -26,6 +26,7 @@ "terminate_when", "terminate_simulation_when", "terminate_after", + "check_time", "in_initial_scenario", "override", "record", @@ -33,6 +34,7 @@ "record_final", "sin", "cos", + "tan", "hypot", "max", "min", @@ -65,9 +67,11 @@ "BottomBackLeft", "BottomBackRight", "RelativeHeading", - "ApparentHeading", + "ApparentHeadingOf", + "ApparentHeadingTo", "RelativePosition", "DistanceFrom", + "MinDistanceFrom", "DistancePast", "Follow", "AngleTo", @@ -84,6 +88,10 @@ "Implies", "VisibleFromOp", "NotVisibleFromOp", + "AheadOfOp", + "BehindOp", + "LeftOfOp", + "RightOfOp", # Primitive types "Vector", "Orientation", @@ -120,6 +128,12 @@ "VerifaiRange", "VerifaiDiscreteRange", "VerifaiOptions", + "VerifaiSampler", + "OptunaRange", + "OptunaDiscreteRange", + "OptunaParameter", + "OptunaOptions", + "OptunaSampler", "TimeSeries", "File", "Files", @@ -201,14 +215,22 @@ ) from scenic.core.dynamics.invocables import BlockConclusion, runTryInterrupt from scenic.core.dynamics.scenarios import DynamicScenario -from scenic.core.external_params import ( - TimeSeries, +from scenic.core.external_params import TimeSeries +from scenic.core.external_params.optuna import ( + OptunaDiscreteRange, + OptunaOptions, + OptunaParameter, + OptunaRange, + OptunaSampler, +) +from scenic.core.external_params.verifai import ( VerifaiDiscreteRange, VerifaiOptions, VerifaiParameter, VerifaiRange, + VerifaiSampler, ) -from scenic.core.geometry import cos, hypot, max, min, sin +from scenic.core.geometry import cos, hypot, max, min, sin, tan from scenic.core.object_types import Mutator, Object, OrientedPoint, Point from scenic.core.regions import ( BoxRegion, @@ -249,6 +271,7 @@ from contextlib import contextmanager import functools import importlib +import math import numbers from pathlib import Path import sys @@ -531,31 +554,27 @@ def executeInRequirement(scenario, boundEgo, values): assert activity == 0 assert not evaluatingRequirement evaluatingRequirement = True - if currentScenario is None: - currentScenario = scenario - clearScenario = True - else: - assert currentScenario is scenario - clearScenario = False - oldEgo = currentScenario._ego - oldObjects = currentScenario._objects - currentScenario._objects = tuple(values[obj] for obj in currentScenario.objects) + with executeInScenario(scenario): + oldEgo = scenario._ego + oldObjects = scenario._objects - if boundEgo: - currentScenario._ego = boundEgo - try: - yield - except RandomControlFlowError as e: - # Such errors should not be possible inside a requirement, since all values - # should have already been sampled: something's gone wrong with our rebinding. - raise RuntimeError("internal error: requirement dependency not sampled") from e - finally: - evaluatingRequirement = False - currentScenario._ego = oldEgo - currentScenario._objects = oldObjects - if clearScenario: - currentScenario = None + scenario._objects = tuple(values[obj] for obj in scenario.objects) + + if boundEgo: + scenario._ego = boundEgo + try: + yield + except RandomControlFlowError as e: + # Such errors should not be possible inside a requirement, since all values + # should have already been sampled: something's gone wrong with our rebinding. + raise AssertionError( + "internal error: requirement dependency not sampled" + ) from e + finally: + evaluatingRequirement = False + scenario._ego = oldEgo + scenario._objects = oldObjects # Dynamic scenarios @@ -837,22 +856,6 @@ def record_final(reqID, value, line, name): makeRequirement(requirements.RequirementType.recordFinal, reqID, value, line, name) -def require_always(reqID, req, line, name): - """Function implementing the 'require always' statement.""" - if not name: - name = f"requirement on line {line}" - makeRequirement(requirements.RequirementType.requireAlways, reqID, req, line, name) - - -def require_eventually(reqID, req, line, name): - """Function implementing the 'require eventually' statement.""" - if not name: - name = f"requirement on line {line}" - makeRequirement( - requirements.RequirementType.requireEventually, reqID, req, line, name - ) - - def terminate_when(reqID, req, line, name): """Function implementing the 'terminate when' statement.""" if not name: @@ -869,23 +872,29 @@ def terminate_simulation_when(reqID, req, line, name): ) -def makeRequirement(ty, reqID, req, line, name, recConfig=None): - if evaluatingRequirement: - raise InvalidScenarioError(f'tried to use "{ty.value}" inside a requirement') - elif currentBehavior is not None: - raise InvalidScenarioError(f'"{ty.value}" inside a behavior on line {line}') - elif currentSimulation is not None: - currentScenario._addDynamicRequirement(ty, req, line, name) - else: # requirement being defined at compile time - currentScenario._addRequirement(ty, reqID, req, line, name, 1, recConfig) +def terminate_after(reqId, req, line, _): + name = "terminate after on line {line}" + makeRequirement(requirements.RequirementType.terminateAfter, reqId, req, line, name) -def terminate_after(timeLimit, terminator=None): +def check_time(timeLimit, terminator=None): + """Returns True if we have exceeded the time limit.""" if not isinstance(timeLimit, (builtins.float, builtins.int)): raise TypeError('"terminate after N" with N not a number') assert terminator in (None, "seconds", "steps") inSeconds = terminator != "steps" - currentScenario._setTimeLimit(timeLimit, inSeconds=inSeconds) + + threshold = timeLimit / simulation().timestep if inSeconds else timeLimit + return currentScenario._elapsedTime > threshold + + +def makeRequirement(ty, reqID, req, line, name, recConfig=None): + if evaluatingRequirement: + raise InvalidScenarioError(f'tried to use "{ty.value}" inside a requirement') + elif currentBehavior is not None: + raise InvalidScenarioError(f'"{ty.value}" inside a behavior on line {line}') + else: + currentScenario._addRequirement(ty, reqID, req, line, name, 1, recConfig) def resample(dist): @@ -1275,7 +1284,7 @@ def RelativeHeading(X, Y=None): return normalizeAngle(X.yaw - Y.yaw) -def ApparentHeading(X, Y=None): +def ApparentHeadingOf(X, Y=None): """The :grammar:`apparent heading of [from ]` operator. If the :grammar:`from ` is omitted, the position of ego is used. @@ -1284,10 +1293,23 @@ def ApparentHeading(X, Y=None): raise TypeError('"apparent heading of X from Y" with X not an OrientedPoint') if Y is None: Y = ego() - Y = toVector(Y, '"relative heading of X from Y" with Y not a vector') + Y = toVector(Y, '"apparent heading of X from Y" with Y not a vector') return apparentHeadingAtPoint(X.position, X.heading, Y) +def ApparentHeadingTo(X, Y=None): + """The :grammar:`apparent heading to [from ]` operator. + + If the :grammar:`from ` is omitted, the ego is used. + """ + X = toVector(X, '"apparent heading to X from Y" with X not a vector') + if Y is None: + Y = ego() + if not isA(Y, OrientedPoint): + raise TypeError('"apparent heading to X from Y" with Y not an OrientedPoint') + return Y.apparentHeadingTo(X) + + def DistanceFrom(X, Y=None): """The :scenic:`distance from {X} to {Y}` polymorphic operator. @@ -1310,6 +1332,18 @@ def DistanceFrom(X, Y=None): return X.distanceTo(Y) +def MinDistanceFrom(X, Y=None): + """The :grammar:`minimum distance from [to ]` operator. + + If the :grammar:`to ` is omitted, the ego is used. + """ + X = toTypes(X, (Object,), '"minimum distance from X to Y" with X not an Object') + if Y is None: + Y = ego() + Y = toTypes(Y, (Object,), '"minimum distance from X to Y" with Y not an Object') + return X.minimumDistanceTo(Y) + + def DistancePast(X, Y=None): """The :grammar:`distance past of ` operator. @@ -1387,6 +1421,44 @@ def NotVisibleFromOp(region, base): return region.difference(base.visibleRegion) +def AheadOfOp(X, Y): + """The :grammar:` ahead of ` operator.""" + X = toVector(X, '"X ahead of Y" with X not a vector') + if not isA(Y, OrientedPoint): + raise TypeError('"X ahead of Y" with Y not an OrientedPoint') + + return math.radians(-89.99) < ApparentHeadingTo(X, Y) < math.radians(89.99) + + +def BehindOp(X, Y): + """The :grammar:` behind ` operator.""" + X = toVector(X, '"X behind Y" with X not a vector') + if not isA(Y, OrientedPoint): + raise TypeError('"X behind Y" with Y not an OrientedPoint') + + return ApparentHeadingTo(X, Y) < -math.radians(90.01) or math.radians( + 90.01 + ) < ApparentHeadingTo(X, Y) + + +def LeftOfOp(X, Y): + """The :grammar:` left of ` operator.""" + X = toVector(X, '"X left of Y" with X not a vector') + if not isA(Y, OrientedPoint): + raise TypeError('"X left of Y" with Y not an OrientedPoint') + + return math.radians(0.01) < ApparentHeadingTo(X, Y) < math.radians(180) + + +def RightOfOp(X, Y): + """The :grammar:` right of ` operator.""" + X = toVector(X, '"X right of Y" with X not a vector') + if not isA(Y, OrientedPoint): + raise TypeError('"X right of Y" with Y not an OrientedPoint') + + return -math.radians(180) < ApparentHeadingTo(X, Y) < math.radians(-0.01) + + def CanSee(X, Y): """The :scenic:`{X} can see {Y}` polymorphic operator. diff --git a/tests/core/test_scenarios.py b/tests/core/test_scenarios.py index 548d5d146..d77c133a0 100644 --- a/tests/core/test_scenarios.py +++ b/tests/core/test_scenarios.py @@ -1,6 +1,10 @@ +import random + import pytest from scenic.core.distributions import Range +from scenic.core.scenarios import Scene +from scenic.core.utils import setSeed from tests.utils import compileScenic @@ -59,3 +63,64 @@ def test_condition_scenario_params_2(): assert all(0.5 <= x <= 0.51 for x in xs) assert any(0.505 <= x for x in xs) assert any(x < 0.505 for x in xs) + + +def test_generateBatch(): + scenario = compileScenic( + """ + ego = new Object facing Range(0, 1) + require ego.heading > 0.5 + """ + ) + scenes, _ = scenario.generateBatch(2, numWorkers=2) + + assert len(scenes) == 2 + assert all(isinstance(scene, Scene) for scene in scenes) + assert all(0 <= scene.objects[0].heading <= 1 for scene in scenes) + assert scenes[0].objects[0].heading != scenes[1].objects[0].heading + + +def test_generateBatch_serialized(): + scenario = compileScenic( + """ + ego = new Object facing Range(0, 1) + require ego.heading > 0.5 + """ + ) + scenesBytes, _ = scenario.generateBatch(2, numWorkers=2, serialized=True) + assert len(scenesBytes) == 2 + assert all(isinstance(b, bytes) for b in scenesBytes) + + scenes = [scenario.sceneFromBytes(b, verify=True) for b in scenesBytes] + assert all(isinstance(scene, Scene) for scene in scenes) + assert all(0 <= scene.objects[0].heading <= 1 for scene in scenes) + assert scenes[0].objects[0].heading != scenes[1].objects[0].heading + + +def test_generateStream_deterministic(): + seed = random.getrandbits(32) + + scenario = compileScenic( + """ + ego = new Object facing Range(0, 1) + require ego.heading > 0.5 + """ + ) + setSeed(seed) + streamA = tuple(scenario.generateStream(8, numWorkers=2, serialized=True)) + setSeed(seed) + streamB = tuple(scenario.generateStream(8, numWorkers=2, serialized=True)) + assert len(streamA) == len(streamB) == 8 + bytesSetA = {result[0] for result in streamA} + bytesSetB = {result[0] for result in streamB} + assert bytesSetA == bytesSetB + + setSeed(seed) + streamA = tuple( + scenario.generateStream(8, numWorkers=2, serialized=True, deterministic=True) + ) + setSeed(seed) + streamB = tuple( + scenario.generateStream(8, numWorkers=2, serialized=True, deterministic=True) + ) + assert streamA == streamB diff --git a/tests/core/test_serialization.py b/tests/core/test_serialization.py index 6ae1d1c95..c4ca8ca0d 100644 --- a/tests/core/test_serialization.py +++ b/tests/core/test_serialization.py @@ -13,8 +13,14 @@ import numpy import pytest -from scenic.core.serialization import SerializationError, Serializer, deterministicHash +from scenic.core.serialization import ( + SerializationError, + Serializer, + deterministicHash, + toOpenScenario, +) from scenic.core.simulators import DivergenceError, DummySimulator +from tests.simulators.metadrive.test_metadrive import getMetadriveSimulator from tests.utils import ( areEquivalent, compileScenic, @@ -507,3 +513,45 @@ class Foo: digest2 = deterministicHash(mapping2) # Non-scalar values should hash in a stable way, independent of identity. assert digest1 == digest2 + + +def test_xosc_export(getMetadriveSimulator): + simulator, openDrivePath, sumoPath = getMetadriveSimulator("Town01") + code = f""" + param map = r'{openDrivePath}' + param sumo_map = r'{sumoPath}' + + model scenic.simulators.metadrive.model + + behavior DriveAndBrakeForPedestrians(): + try: + do FollowLaneBehavior() + interrupt when withinDistanceToAnyPedestrians(self, 10): + take SetThrottleAction(0), SetBrakeAction(1) + + behavior CrossRoad(): + while distance from self to ego > 15: + wait + take SetWalkingDirectionAction(self.heading), SetWalkingSpeedAction(1) + + ego = new Car with behavior DriveAndBrakeForPedestrians() + + rightCurb = ego.laneGroup.curb + spot = new OrientedPoint on visible rightCurb + + parkedCar = new Car right of spot by 1, with regionContainedIn None + + require distance from ego to parkedCar > 30 + + new Pedestrian ahead of parkedCar by 3, + facing 90 deg relative to parkedCar, + with behavior CrossRoad() + + terminate after 30 seconds + """ + + scenario = compileScenic(code, mode2D=True, params={"map": openDrivePath}) + scene, _ = scenario.generate() + simulationResult = simulator.simulate(scene) + assert simulationResult is not None + xosc_scenario = toOpenScenario(simulationResult, scenario, scene) diff --git a/tests/core/test_simulators.py b/tests/core/test_simulators.py index 149c1cad1..0e9d1d60e 100644 --- a/tests/core/test_simulators.py +++ b/tests/core/test_simulators.py @@ -1,7 +1,24 @@ +import itertools +import random + import pytest -from scenic.core.simulators import DummySimulation, DummySimulator, Simulation -from tests.utils import compileScenic, sampleResultFromScene, sampleSceneFrom +import scenic +from scenic.core.simulators import ( + DummySimulation, + DummySimulator, + Simulation, + SimulatorGroup, + TerminatedSimulationException, +) +from tests.utils import ( + RejectSimulationException, + checkVeneerIsInactive, + compileScenic, + sampleResult, + sampleResultFromScene, + sampleSceneFrom, +) def test_old_style_simulator(): @@ -35,6 +52,29 @@ def test_simulator_destruction(): assert "destroy() called twice" in str(e) +def test_simulator_stepped(): + simulator = DummySimulator() + scene = sampleSceneFrom("ego = new Object") + + with simulator.simulateStepped(scene, maxSteps=5) as simulation: + while simulation.result is None: + simulation.advance() + + assert simulation.result is not None + assert simulation.currentTime == 5 + + # advance() should do nothing but raise an exception + # if the simulation is already terminated + with pytest.raises(TerminatedSimulationException): + simulation.advance() + + assert simulation.currentTime == 5 + + # Ensure all values are preserved after leaving the context manager + assert simulation.result is not None + assert simulation.currentTime == 5 + + def test_simulator_set_property(): class TestSimulation(DummySimulation): def createObjectInSimulator(self, obj): @@ -94,3 +134,186 @@ class TestObj: simulator = TestSimulator() with pytest.raises(RuntimeError): result = simulator.simulate(scene, maxSteps=2) + + +@pytest.mark.slow +def test_simulator_group(): + scenario = compileScenic( + """ + behavior Foo(): + while True: + require Range(0,1) < 0.99 + wait + + new Object with behavior Foo() + """ + ) + + for numWorkers, serialized, scene_stream, sim_stream in itertools.product( + [1, 2], [True, False], [True, False], [True, False] + ): + if scene_stream: + scenes = scenario.generateStream( + 200, numWorkers=numWorkers, serialized=serialized, iterationCount=False + ) + else: + scenes, _ = scenario.generateBatch( + 200, numWorkers=numWorkers, serialized=serialized + ) + + sim_group = SimulatorGroup( + numWorkers=2, simulatorClass=DummySimulator, mute=False + ) + + simulate_params = {"maxSteps": 10} + + if sim_stream: + results = tuple( + result + for _, result in sim_group.simulateStream( + scenario, + scenes, + simulateParams=simulate_params, + serialized=serialized, + ) + ) + else: + results = sim_group.simulateBatch( + scenario, scenes, simulateParams=simulate_params, serialized=serialized + ) + + assert any(val is None for val in results) + assert any(val is not None for val in results) + + +def test_simulator_group_deterministic(): + scenario = compileScenic( + """ + behavior Foo(): + while True: + require Range(0,1) < 0.99 + wait + + new Object with behavior Foo() + """ + ) + + seed = random.getrandbits(32) + + scenic.setSeed(seed) + scenes, _ = scenario.generateBatch(200, serialized=True) + + sim_group = SimulatorGroup(numWorkers=4, simulatorClass=DummySimulator, mute=False) + simulate_params = {"maxSteps": 10} + + results1 = tuple( + result + for _, result in sim_group.simulateStream( + scenario, scenes, simulateParams=simulate_params, deterministic=True + ) + ) + + scenic.setSeed(seed) + scenes, _ = scenario.generateBatch(200, serialized=True) + + sim_group = SimulatorGroup(numWorkers=4, simulatorClass=DummySimulator, mute=False) + simulate_params = {"maxSteps": 10} + + results2 = tuple( + result + for _, result in sim_group.simulateStream( + scenario, scenes, simulateParams=simulate_params, deterministic=True + ) + ) + + assert len(results1) == len(results2) + assert all((v1 is None) == (v2 is None) for v1, v2 in zip(results1, results2)) + + +def test_simulator_createObjectInSimulator_error_cleanup(): + destroy_called = False + + class TestException(Exception): + pass + + class TestSimulator(DummySimulator): + def createSimulation(self, scene, **kwargs): + return TestSimulation(scene, drift=self.drift, **kwargs) + + class TestSimulation(DummySimulation): + def createObjectInSimulator(self, obj): + raise TestException() + + def destroy(self): + nonlocal destroy_called + destroy_called = True + + simulator = TestSimulator() + checkVeneerIsInactive() + with pytest.raises(TestException): + sampleResult(compileScenic("ego = new Object"), simulator=simulator) + checkVeneerIsInactive() + + assert destroy_called + + +def test_simulator_step_error_cleanup(): + destroy_called = False + + class TestException(Exception): + pass + + class TestSimulator(DummySimulator): + def createSimulation(self, scene, **kwargs): + return TestSimulation(scene, drift=self.drift, **kwargs) + + class TestSimulation(DummySimulation): + def step(self): + raise TestException() + + def destroy(self): + nonlocal destroy_called + destroy_called = True + + simulator = TestSimulator() + checkVeneerIsInactive() + with pytest.raises(TestException): + sampleResult(compileScenic("ego = new Object"), simulator=simulator) + checkVeneerIsInactive() + + assert destroy_called + + +def test_simulator_rejection_cleanup(): + destroy_called = False + + class TestException(Exception): + pass + + class TestSimulator(DummySimulator): + def createSimulation(self, scene, **kwargs): + return TestSimulation(scene, drift=self.drift, **kwargs) + + class TestSimulation(DummySimulation): + def destroy(self): + nonlocal destroy_called + destroy_called = True + + simulator = TestSimulator() + checkVeneerIsInactive() + with pytest.raises(RejectSimulationException): + sampleResult( + compileScenic( + """ + ego = new Object + monitor Foo(): + require False + wait + require monitor Foo() + """ + ), + simulator=simulator, + ) + checkVeneerIsInactive() + + assert destroy_called diff --git a/tests/core/test_vectors.py b/tests/core/test_vectors.py index 82f8be7ff..b9768b1bd 100644 --- a/tests/core/test_vectors.py +++ b/tests/core/test_vectors.py @@ -69,3 +69,24 @@ def test_distribution_method_encapsulation_lazy(): assert not needsLazyEvaluation(evpt) assert isinstance(evpt, VectorMethodDistribution) assert evpt.method is underlyingFunction(vf.followFrom) + + +@pytest.mark.skipif( + sys.version_info < (3, 10), reason="Pairwise requires Python 3.10 or higher." +) +def test_vf_follow(): + vf = VectorField( + "Foo", lambda pos: 0 if int(pos.x + pos.y) % 2 == 0 else (math.radians(-90)) + ) + + start_pt = Vector(0, 0) + end_pt = vf.followFrom(start_pt, 5, stepSize=1) + traj = vf.followFromTrajectory(start_pt, 5, stepSize=1) + + assert traj.points[-1] == pytest.approx(end_pt) + + for pt_a, pt_b in itertools.pairwise(traj.points): + if int(sum(pt_a)) % 2 == 0: + assert pt_a + Vector(0, 1) == pytest.approx(pt_b) + else: + assert pt_a + Vector(1, 0) == pytest.approx(pt_b) diff --git a/tests/domains/driving/test_network.py b/tests/domains/driving/test_network.py index b03061cd2..e3c6ee460 100644 --- a/tests/domains/driving/test_network.py +++ b/tests/domains/driving/test_network.py @@ -1,8 +1,11 @@ from pathlib import Path +import random import pytest +import shapely from scenic.core.distributions import RejectionException +from scenic.core.regions import toPolygon from scenic.domains.driving.roads import Intersection, Network from tests.domains.driving.conftest import mapFolder @@ -257,6 +260,16 @@ def test_sidewalk(network): assert network.elementAt(pt) is sw +def test_laneGroup_lane_order(network): + for _ in range(30): + lg = random.choice(network.laneGroups) + lane_0_dist = shapely.distance(toPolygon(lg.lanes[0]), toPolygon(lg.curb)) + lane_distances = [ + shapely.distance(toPolygon(lane), toPolygon(lg.curb)) for lane in lg.lanes + ] + assert all(lane_dist >= lane_0_dist - 0.1 for lane_dist in lane_distances) + + # --- Tests for cached network pickles --- diff --git a/tests/simulators/metadrive/test_metadrive.py b/tests/simulators/metadrive/test_metadrive.py index 1c1ea9ad1..4fe573d2e 100644 --- a/tests/simulators/metadrive/test_metadrive.py +++ b/tests/simulators/metadrive/test_metadrive.py @@ -84,7 +84,9 @@ def test_pickle(loadLocalScenario): def getMetadriveSimulator(getAssetPath): base = getAssetPath("maps/CARLA") - def _getMetadriveSimulator(town, *, render=False, render3D=False, **kwargs): + def _getMetadriveSimulator( + town, *, render=False, render3D=False, real_time=False, **kwargs + ): openDrivePath = os.path.join(base, f"{town}.xodr") sumoPath = os.path.join(base, f"{town}.net.xml") simulator = MetaDriveSimulator( @@ -92,6 +94,7 @@ def _getMetadriveSimulator(town, *, render=False, render3D=False, **kwargs): xodr_map=openDrivePath, render=render, render3D=render3D, + real_time=real_time, **kwargs, ) return simulator, openDrivePath, sumoPath diff --git a/tests/simulators/newtonian/test_newtonian.py b/tests/simulators/newtonian/test_newtonian.py index 98561c527..578f82b6c 100644 --- a/tests/simulators/newtonian/test_newtonian.py +++ b/tests/simulators/newtonian/test_newtonian.py @@ -4,6 +4,7 @@ from PIL import Image as IPImage import pytest +from scenic.core.simulators import TerminationType from scenic.domains.driving.roads import Network from scenic.simulators.newtonian import NewtonianSimulator from tests.utils import compileScenic, pickle_test, sampleScene, tryPickling @@ -113,3 +114,42 @@ def test_pedestrian_velocity_vector(getAssetPath): # Expect movement northeast (positive dx and dy) assert dx > 0.1, f"Expected positive x movement (east), got dx = {dx}" assert dy > 0.1, f"Expected positive y movement (north), got dy = {dy}" + + +## Pedestrian Behaviors +@pytest.mark.slow +def test_pedestrian_sidewalk_conflict(getAssetPath): + mapPath = getAssetPath("maps/CARLA/Town01.xodr") + + code = f""" + param map = r'{mapPath}' + model scenic.simulators.newtonian.driving_model + + targetSidewalk = Uniform(*network.sidewalks) + + pedASpeed = Range(0.9, 1.8) + pedBSpeed = Range(0.9, 1.8) + + pedA = new Pedestrian at targetSidewalk.centerline.start, + with behavior WalkTo(targetSidewalk.centerline.end, targetSpeed=pedASpeed) + + pedB = new Pedestrian at targetSidewalk.centerline.end, + with behavior WalkTo(targetSidewalk.centerline.start, targetSpeed=pedBSpeed) + + param minPedSpeed = min(pedASpeed, pedBSpeed) + param pedDistance = targetSidewalk.centerline.length + + require targetSidewalk.centerline.length >= 10 + + terminate when ((distance from pedA to targetSidewalk.centerline.end) < 0.1 + and (distance from pedB to targetSidewalk.centerline.start) < 0.1) + """ + scenario = compileScenic(code, mode2D=True) + for _ in range(5): + scene, _ = scenario.generate(maxIterations=100) + simulator = NewtonianSimulator(render=False) + maxSteps = int( + 2 * (scene.params["pedDistance"] / scene.params["minPedSpeed"]) / 0.1 + ) + simulation = simulator.simulate(scene, maxSteps=maxSteps) + assert simulation.result.terminationType == TerminationType.scenarioComplete diff --git a/tests/syntax/helper_record.scenic b/tests/syntax/helper_record.scenic new file mode 100644 index 000000000..d77654391 --- /dev/null +++ b/tests/syntax/helper_record.scenic @@ -0,0 +1 @@ +record initial 1 as "foo" diff --git a/tests/syntax/helper_require_monitor.scenic b/tests/syntax/helper_require_monitor.scenic new file mode 100644 index 000000000..033f619dd --- /dev/null +++ b/tests/syntax/helper_require_monitor.scenic @@ -0,0 +1,5 @@ +monitor Bar(): + wait for 1 steps + terminate + +require monitor Bar() diff --git a/tests/syntax/helper_terminate.scenic b/tests/syntax/helper_terminate.scenic new file mode 100644 index 000000000..62fa24799 --- /dev/null +++ b/tests/syntax/helper_terminate.scenic @@ -0,0 +1 @@ +terminate after 1 steps diff --git a/tests/syntax/test_basic.py b/tests/syntax/test_basic.py index 8c3d01483..aa612d6c2 100644 --- a/tests/syntax/test_basic.py +++ b/tests/syntax/test_basic.py @@ -411,3 +411,14 @@ def test_simulator_name_binding_executes(): ) ego = sampleEgo(scenario) assert ego.foo == 7 + + +def test_setSeed(): + scenic.setSeed(10) + p1 = sampleParamPFrom("param p = Range(0, 1)") + scenic.setSeed(10) + p2 = sampleParamPFrom("param p = Range(0, 1)") + p3 = sampleParamPFrom("param p = Range(0, 1)") + + assert p1 == p2 + assert p1 != p3 diff --git a/tests/syntax/test_compiler.py b/tests/syntax/test_compiler.py index 597b85b55..ceac75760 100644 --- a/tests/syntax/test_compiler.py +++ b/tests/syntax/test_compiler.py @@ -1710,20 +1710,58 @@ def test_terminate_simulation_when(self): assert False def test_terminate_after_seconds(self): - node, _ = compileScenicAST(TerminateAfter(Seconds(Constant(10)))) + node, _ = compileScenicAST(TerminateAfter(Seconds(Constant(10)), lineno=2)) match node: case Expr( - Call(Name("terminate_after"), [Constant(10), Constant("seconds")], []) + Call( + Name("terminate_after"), + [ + Constant(0), # reqId + Call( + Name("AtomicProposition"), + [ + Lambda( + arguments(), + Call( + Name("check_time"), + [Constant(10), Constant("seconds")], + ), + ) + ], + ), + Constant(2), # lineno + Constant(None), # name + ], + ) ): assert True case _: assert False def test_terminate_after_steps(self): - node, _ = compileScenicAST(TerminateAfter(Steps(Constant(20)))) + node, _ = compileScenicAST(TerminateAfter(Steps(Constant(20)), lineno=2)) match node: case Expr( - Call(Name("terminate_after"), [Constant(20), Constant("steps")], []) + Call( + Name("terminate_after"), + [ + Constant(0), # reqId + Call( + Name("AtomicProposition"), + [ + Lambda( + arguments(), + Call( + Name("check_time"), + [Constant(20), Constant("steps")], + ), + ) + ], + ), + Constant(2), # lineno + Constant(None), # name + ], + ) ): assert True case _: @@ -2075,18 +2113,34 @@ def test_relative_heading_op_base(self): case _: assert False - def test_apparent_heading_op(self): - node, _ = compileScenicAST(ApparentHeadingOp(Name("X"))) + def test_apparent_heading_of_op(self): + node, _ = compileScenicAST(ApparentHeadingOfOp(Name("X"))) + match node: + case Call(Name("ApparentHeadingOf"), [Name("X")]): + assert True + case _: + assert False + + def test_apparent_heading_of_op_base(self): + node, _ = compileScenicAST(ApparentHeadingOfOp(Name("X"), Name("Y"))) + match node: + case Call(Name("ApparentHeadingOf"), [Name("X")], [keyword("Y", Name("Y"))]): + assert True + case _: + assert False + + def test_apparent_heading_to_op(self): + node, _ = compileScenicAST(ApparentHeadingToOp(Name("X"))) match node: - case Call(Name("ApparentHeading"), [Name("X")]): + case Call(Name("ApparentHeadingTo"), [Name("X")]): assert True case _: assert False - def test_apparent_heading_op_base(self): - node, _ = compileScenicAST(ApparentHeadingOp(Name("X"), Name("Y"))) + def test_apparent_heading_to_op_base(self): + node, _ = compileScenicAST(ApparentHeadingToOp(Name("X"), Name("Y"))) match node: - case Call(Name("ApparentHeading"), [Name("X")], [keyword("Y", Name("Y"))]): + case Call(Name("ApparentHeadingTo"), [Name("X")], [keyword("Y", Name("Y"))]): assert True case _: assert False @@ -2107,6 +2161,22 @@ def test_distance_to_op_from(self): case _: assert False + def test_min_distance_to_op(self): + node, _ = compileScenicAST(MinDistanceFromOp(Name("X"), None)) + match node: + case Call(Name("MinDistanceFrom"), [Name("X")], []): + assert True + case _: + assert False + + def test_min_distance_to_op_from(self): + node, _ = compileScenicAST(MinDistanceFromOp(Name("X"), Name("Y"))) + match node: + case Call(Name("MinDistanceFrom"), [Name("X")], [keyword("Y", Name("Y"))]): + assert True + case _: + assert False + def test_distance_past_op(self): node, _ = compileScenicAST(DistancePastOp(Name("X"))) match node: @@ -2290,6 +2360,38 @@ def test_offset_along_op(self): case _: assert False + def test_ahead_of_op(self): + node, _ = compileScenicAST(AheadOfOp(Name("X"), Name("Y"))) + match node: + case Call(Name("AheadOfOp"), [Name("X"), Name("Y")]): + assert True + case _: + assert False + + def test_behind_op(self): + node, _ = compileScenicAST(BehindOp(Name("X"), Name("Y"))) + match node: + case Call(Name("BehindOp"), [Name("X"), Name("Y")]): + assert True + case _: + assert False + + def test_left_of_op(self): + node, _ = compileScenicAST(LeftOfOp(Name("X"), Name("Y"))) + match node: + case Call(Name("LeftOfOp"), [Name("X"), Name("Y")]): + assert True + case _: + assert False + + def test_right_of_op(self): + node, _ = compileScenicAST(RightOfOp(Name("X"), Name("Y"))) + match node: + case Call(Name("RightOfOp"), [Name("X"), Name("Y")]): + assert True + case _: + assert False + def test_can_see_op(self): node, _ = compileScenicAST(CanSeeOp(Name("X"), Name("Y"))) match node: diff --git a/tests/syntax/test_dynamics.py b/tests/syntax/test_dynamics.py index 4699a2921..ae3c98479 100644 --- a/tests/syntax/test_dynamics.py +++ b/tests/syntax/test_dynamics.py @@ -188,6 +188,25 @@ def test_behavior_take_empty_tuple(): assert tuple(actions) == (None, 7) +def test_parallel_behaviors(): + scenario = compileScenic( + """ + behavior Fizz(): + take "Fizz" + + behavior Buzz(): + take "Buzz" + + behavior Foo(): + take "Start" + do Buzz(), Fizz() + ego = new Object with behavior Foo + """ + ) + actions = sampleEgoActions(scenario, maxSteps=2, singleAction=False) + assert tuple(actions) == (("Start",), ("Buzz", "Fizz")) + + # Various errors @@ -557,6 +576,26 @@ def test_terminate_when(): assert tuple(actions) == (1, 2) +def test_terminate_minimum_time(): + scenario = compileScenic( + """ + behavior Foo(): + i = 0 + while True: + i += 1 + take i + ego = new Object with behavior Foo + terminate after 6 seconds + terminate after 4 steps + terminate after 5 steps + """ + ) + actions = sampleEgoActions(scenario, maxSteps=3) + assert tuple(actions) == (1, 2, 3) + actions = sampleEgoActions(scenario, maxSteps=7) + assert tuple(actions) == (1, 2, 3, 4) + + # Reuse @@ -770,19 +809,6 @@ def test_behavior_invoke_mistyped(): sampleActions(scenario) -def test_behavior_invoke_multiple(): - with pytest.raises(ScenicSyntaxError): - compileScenic( - """ - behavior Foo(): - take 5 - behavior Bar(): - do Foo(), Foo() - ego = new Object with behavior Bar - """ - ) - - def test_behavior_tuple_invalid(): scenario = compileScenic( """ @@ -2223,6 +2249,7 @@ def test_termination_reason_monitor(): ## Recording +# (see also `test_recording.py`) def test_record(): diff --git a/tests/syntax/test_imports.py b/tests/syntax/test_imports.py index c5b3ca0a5..88b3551f1 100644 --- a/tests/syntax/test_imports.py +++ b/tests/syntax/test_imports.py @@ -13,7 +13,7 @@ from scenic import scenarioFromFile from scenic.core.errors import ScenicSyntaxError from scenic.syntax.translator import InvalidScenarioError -from tests.utils import compileScenic, sampleScene, sampleSceneFrom +from tests.utils import compileScenic, sampleResult, sampleScene, sampleSceneFrom def test_import_top_absolute(request): @@ -77,6 +77,51 @@ def test_inherit_requirements(runLocally): assert constrainedObj.position.x > 0 +def test_inherit_records(runLocally): + with runLocally(): + scenario = compileScenic( + """ + import helper_record + ego = new Object + """ + ) + + result = sampleResult(scenario, maxSteps=1) + assert "foo" in result.records + assert result.records["foo"] == 1 + + +def test_inherit_terminate(runLocally): + with runLocally(): + scenario = compileScenic( + """ + import helper_terminate + ego = new Object + record 1 as "foo" + terminate after 4 steps + """ + ) + + result = sampleResult(scenario, maxSteps=5) + assert "foo" in result.records + assert result.records["foo"] == [(0, 1), (1, 1)] + + +def test_inherit_require_monitor(runLocally): + with runLocally(): + scenario = compileScenic( + """ + import helper_require_monitor + ego = new Object + record 1 as "foo" + """ + ) + + result = sampleResult(scenario, maxSteps=5) + assert "foo" in result.records + assert result.records["foo"] == [(0, 1), (1, 1)] + + def test_inherit_constructors(runLocally): with runLocally(): scenario = compileScenic("from helper import Caerbannog\n" "ego = new Caerbannog") diff --git a/tests/syntax/test_modular.py b/tests/syntax/test_modular.py index c77a54fce..21f1a3425 100644 --- a/tests/syntax/test_modular.py +++ b/tests/syntax/test_modular.py @@ -572,6 +572,24 @@ def test_subscenario_require_eventually(): assert result is None +def test_subscenario_require_eventually_2(): + """Variant of the above test using `terminate when` instead of `terminate after`.""" + scenario = compileScenic( + """ + scenario Main(): + compose: + do Sub() + wait + scenario Sub(): + ego = new Object + require eventually simulation().currentTime == 2 + terminate when simulation().currentTime == 1 + """ + ) + result = sampleResultOnce(scenario, maxSteps=2) + assert result is None + + def test_subscenario_require_monitor(): """Test that monitors invoked in subscenarios terminate with the subscenario.""" scenario = compileScenic( @@ -595,8 +613,42 @@ def test_subscenario_require_monitor(): assert len(result.trajectory) == 4 +def test_subscenario_record(): + scenario = compileScenic( + """ + scenario Main(): + setup: + record initial simulation().currentTime as mainInitial + record final simulation().currentTime as mainFinal + record simulation().currentTime as mainTime + compose: + wait for 2 steps + do Sub() + wait + scenario Sub(): + ego = new Object + record initial -simulation().currentTime as subInitial + record final -simulation().currentTime as subFinal + record -simulation().currentTime as subNegTime + terminate after 2 steps + """ + ) + result = sampleResult(scenario, maxSteps=5) + records = result.records + assert records["mainInitial"] == 0 + assert records["mainFinal"] == 5 + assert tuple(records["mainTime"]) == ((0, 0), (1, 1), (2, 2), (3, 3), (4, 4), (5, 5)) + assert records["subInitial"] == -2 + assert records["subFinal"] == -4 + assert tuple(records["subNegTime"]) == ((2, -2), (3, -3), (4, -4)) + + def test_subscenario_terminate_when(): - """Test that 'terminate when' and 'require' are properly handled.""" + """Test that 'terminate when' is properly handled. + + In particular, this catches a bug where `terminate when` in a subscenario was + interpreted as defining a requirement instead of a termination condition. + """ scenario = compileScenic( """ scenario Main(): @@ -605,12 +657,12 @@ def test_subscenario_terminate_when(): wait scenario Sub(): ego = new Object - require eventually simulation().currentTime == 2 terminate when simulation().currentTime == 1 """ ) - result = sampleResultOnce(scenario, maxSteps=2) - assert result is None + result = sampleResultOnce(scenario, maxSteps=3) + assert result is not None + assert len(result.trajectory) == 3 def test_subscenario_terminate_with_parent(): diff --git a/tests/syntax/test_operators.py b/tests/syntax/test_operators.py index 7c405c789..def3f15f8 100644 --- a/tests/syntax/test_operators.py +++ b/tests/syntax/test_operators.py @@ -43,8 +43,8 @@ def test_relative_heading_from(): assert ego.heading == pytest.approx(math.radians(70 + 10)) -# Apparent heading -def test_apparent_heading(): +# Apparent heading of +def test_apparent_heading_of(): p = sampleParamPFrom( """ ego = new Object facing 30 deg @@ -55,7 +55,7 @@ def test_apparent_heading(): assert p == pytest.approx(math.radians(65 + 45)) -def test_apparent_heading_no_ego(): +def test_apparent_heading_of_no_ego(): with pytest.raises(InvalidScenarioError): compileScenic( """ @@ -65,7 +65,7 @@ def test_apparent_heading_no_ego(): ) -def test_apparent_heading_from(): +def test_apparent_heading_of_from(): ego = sampleEgoFrom( """ OP = new OrientedPoint at 10@15, facing -60 deg @@ -75,6 +75,38 @@ def test_apparent_heading_from(): assert ego.heading == pytest.approx(math.radians(-60 - 45)) +def test_apparent_heading_to(): + p = sampleParamPFrom( + """ + ego = new Object facing 30 deg + other = new Object facing 65 deg, at 10@10 + param p = apparent heading to other + """ + ) + assert p == pytest.approx(math.radians(-30 - 45)) + + +def test_apparent_heading_to_no_ego(): + with pytest.raises(InvalidScenarioError): + compileScenic( + """ + other = new Object + ego = new Object at 2@2, facing apparent heading to other + """ + ) + + +def test_apparent_heading_to_from(): + p = sampleParamPFrom( + """ + foo = new Object facing 30 deg + other = new Object facing 65 deg, at 10@10 + param p = apparent heading to other from foo + """ + ) + assert p == pytest.approx(math.radians(-30 - 45)) + + # Angle def test_angle(): p = sampleParamPFrom( @@ -188,6 +220,57 @@ def test_distance_to_region(): assert p == pytest.approx(2) +# Minimum Distance +def test_minimum_distance(): + p = sampleParamPFrom( + """ + ego = new Object at (1.5, 2, 2.5), + with width 1, with length 2, with height 3 + other = new Object at (-10, -10, -10), + with width 2, with length 2, with height 2 + param p = minimum distance to other + """ + ) + assert p == pytest.approx(math.hypot(10, 10, 10)) + + +def test_minimum_distance_from(): + p = sampleParamPFrom( + """ + foo = new Object at (1.5, 2, 2.5), + with width 1, with length 2, with height 3 + bar = new Object at (-10, -10, -10), + with width 2, with length 2, with height 2 + param p = minimum distance from foo to bar + """ + ) + assert p == pytest.approx(math.hypot(10, 10, 10)) + + +def test_minimum_distance_no_ego(): + with pytest.raises(InvalidScenarioError): + sampleParamPFrom( + """ + other = new Object at (-10, -10, -10), + with width 2, with length 2, with height 2 + param p = minimum distance to other + """ + ) + + +def test_minimum_distance_2d(): + p = sampleParamPFrom( + """ + ego = new Object at (1.5, 2), + with width 1, with length 2 + other = new Object at (-10, -10), + with width 2, with length 2 + param p = minimum distance to other + """ + ) + assert p == pytest.approx(math.hypot(10, 10)) + + # Distance past @@ -602,6 +685,62 @@ def test_intersects_diff_z(): assert p == (True, False, False) +def test_ahead_of_op(): + p_vals = [ + sampleParamPFrom( + f""" + ego = new Object + foo = new Point offset along {i*45} deg by 0@1 + param p = foo ahead of ego + """ + ) + for i in range(8) + ] + assert p_vals == [True, True, False, False, False, False, False, True] + + +def test_behind_op(): + p_vals = [ + sampleParamPFrom( + f""" + ego = new Object + foo = new Point offset along {i*45} deg by 0@1 + param p = foo behind ego + """ + ) + for i in range(8) + ] + assert p_vals == [False, False, False, True, True, True, False, False] + + +def test_left_of_op(): + p_vals = [ + sampleParamPFrom( + f""" + ego = new Object + foo = new Point offset along {i*45} deg by 0@1 + param p = foo left of ego + """ + ) + for i in range(8) + ] + assert p_vals == [False, True, True, True, False, False, False, False] + + +def test_right_of_op(): + p_vals = [ + sampleParamPFrom( + f""" + ego = new Object + foo = new Point offset along {i*45} deg by 0@1 + param p = foo right of ego + """ + ) + for i in range(8) + ] + assert p_vals == [False, False, False, False, False, True, True, True] + + ## Heading operators diff --git a/tests/syntax/test_parser.py b/tests/syntax/test_parser.py index b259b2ad0..a1415576c 100644 --- a/tests/syntax/test_parser.py +++ b/tests/syntax/test_parser.py @@ -2218,20 +2218,20 @@ def test_relative_heading_from(self): def test_relative_heading_precedence(self, code, expected): assert_equal_source_ast(code, expected) - def test_apparent_heading(self): + def test_apparent_heading_of(self): mod = parse_string_helper("apparent heading of x") stmt = mod.body[0] match stmt: - case Expr(ApparentHeadingOp(Name("x"))): + case Expr(ApparentHeadingOfOp(Name("x"))): assert True case _: assert False - def test_apparent_heading_from(self): + def test_apparent_heading_of_from(self): mod = parse_string_helper("apparent heading of x from y") stmt = mod.body[0] match stmt: - case Expr(ApparentHeadingOp(Name("x"), Name("y"))): + case Expr(ApparentHeadingOfOp(Name("x"), Name("y"))): assert True case _: assert False @@ -2241,27 +2241,27 @@ def test_apparent_heading_from(self): [ ( "apparent heading of apparent heading of A from B", - ApparentHeadingOp( - ApparentHeadingOp(Name("A", Load()), Name("B", Load())) + ApparentHeadingOfOp( + ApparentHeadingOfOp(Name("A", Load()), Name("B", Load())) ), ), ( "apparent heading of apparent heading of A from B from C", - ApparentHeadingOp( - ApparentHeadingOp(Name("A", Load()), Name("B", Load())), + ApparentHeadingOfOp( + ApparentHeadingOfOp(Name("A", Load()), Name("B", Load())), Name("C", Load()), ), ), ( "apparent heading of A from apparent heading of B from C", - ApparentHeadingOp( + ApparentHeadingOfOp( Name("A", Load()), - ApparentHeadingOp(Name("B", Load()), Name("C", Load())), + ApparentHeadingOfOp(Name("B", Load()), Name("C", Load())), ), ), ( "apparent heading of A << B from C", - ApparentHeadingOp( + ApparentHeadingOfOp( BinOp(Name("A", Load()), LShift(), Name("B", Load())), Name("C", Load()), ), @@ -2269,32 +2269,124 @@ def test_apparent_heading_from(self): ( "apparent heading of A from B << C", BinOp( - ApparentHeadingOp(Name("A", Load()), Name("B", Load())), + ApparentHeadingOfOp(Name("A", Load()), Name("B", Load())), LShift(), Name("C", Load()), ), ), ( "apparent heading of A + B from C", - ApparentHeadingOp( + ApparentHeadingOfOp( BinOp(Name("A", Load()), Add(), Name("B", Load())), Name("C", Load()), ), ), ( "apparent heading of A from B + C", - ApparentHeadingOp( + ApparentHeadingOfOp( Name("A", Load()), BinOp(Name("B", Load()), Add(), Name("C", Load())), ), ), ( "apparent heading of A << B", - BinOp(ApparentHeadingOp(Name("A", Load())), LShift(), Name("B", Load())), + BinOp( + ApparentHeadingOfOp(Name("A", Load())), LShift(), Name("B", Load()) + ), ), ( "apparent heading of A + B", - ApparentHeadingOp(BinOp(Name("A", Load()), Add(), Name("B", Load()))), + ApparentHeadingOfOp(BinOp(Name("A", Load()), Add(), Name("B", Load()))), + ), + ], + ) + def test_apparent_heading_precedence(self, code, expected): + assert_equal_source_ast(code, expected) + + def test_apparent_heading_to_from(self): + mod = parse_string_helper("apparent heading to x from y") + stmt = mod.body[0] + match stmt: + case Expr(ApparentHeadingToOp(Name("x"), Name("y"))): + assert True + case _: + assert False + + @pytest.mark.parametrize( + "code,expected", + [ + ( + "apparent heading to apparent heading to A from B", + ApparentHeadingToOp( + ApparentHeadingToOp(Name("A", Load()), Name("B", Load())) + ), + ), + ( + "apparent heading to apparent heading to A from B from C", + ApparentHeadingToOp( + ApparentHeadingToOp(Name("A", Load()), Name("B", Load())), + Name("C", Load()), + ), + ), + ( + "apparent heading to A from apparent heading to B from C", + ApparentHeadingToOp( + Name("A", Load()), + ApparentHeadingToOp(Name("B", Load()), Name("C", Load())), + ), + ), + ( + "apparent heading of A from apparent heading to B from C", + ApparentHeadingOfOp( + Name("A", Load()), + ApparentHeadingToOp(Name("B", Load()), Name("C", Load())), + ), + ), + ( + "apparent heading to A from apparent heading of B from C", + ApparentHeadingToOp( + Name("A", Load()), + ApparentHeadingOfOp(Name("B", Load()), Name("C", Load())), + ), + ), + ( + "apparent heading to A << B from C", + ApparentHeadingToOp( + BinOp(Name("A", Load()), LShift(), Name("B", Load())), + Name("C", Load()), + ), + ), + ( + "apparent heading to A from B << C", + BinOp( + ApparentHeadingToOp(Name("A", Load()), Name("B", Load())), + LShift(), + Name("C", Load()), + ), + ), + ( + "apparent heading to A + B from C", + ApparentHeadingToOp( + BinOp(Name("A", Load()), Add(), Name("B", Load())), + Name("C", Load()), + ), + ), + ( + "apparent heading to A from B + C", + ApparentHeadingToOp( + Name("A", Load()), + BinOp(Name("B", Load()), Add(), Name("C", Load())), + ), + ), + ( + "apparent heading to A << B", + BinOp( + ApparentHeadingToOp(Name("A", Load())), LShift(), Name("B", Load()) + ), + ), + ( + "apparent heading to A + B", + ApparentHeadingToOp(BinOp(Name("A", Load()), Add(), Name("B", Load()))), ), ], ) @@ -2337,6 +2429,33 @@ def test_distance_to_from(self): case _: assert False + def test_min_distance_to(self): + mod = parse_string_helper("minimum distance to x") + stmt = mod.body[0] + match stmt: + case Expr(MinDistanceFromOp(Name("x"), None)): + assert True + case _: + assert False + + def test_min_distance_from_to(self): + mod = parse_string_helper("minimum distance from x to y") + stmt = mod.body[0] + match stmt: + case Expr(MinDistanceFromOp(Name("x"), Name("y"))): + assert True + case _: + assert False + + def test_min_distance_to_from(self): + mod = parse_string_helper("minimum distance to x from y") + stmt = mod.body[0] + match stmt: + case Expr(MinDistanceFromOp(Name("x"), Name("y"))): + assert True + case _: + assert False + @pytest.mark.parametrize( "code,expected", [ @@ -3005,6 +3124,42 @@ def test_can_see(self): case _: assert False + def test_ahead_of(self): + mod = parse_string_helper("x ahead of y ") + stmt = mod.body[0] + match stmt: + case Expr(AheadOfOp(Name("x"), Name("y"))): + assert True + case _: + assert False + + def test_behind(self): + mod = parse_string_helper("x behind y ") + stmt = mod.body[0] + match stmt: + case Expr(BehindOp(Name("x"), Name("y"))): + assert True + case _: + assert False + + def test_left_of(self): + mod = parse_string_helper("x left of y ") + stmt = mod.body[0] + match stmt: + case Expr(LeftOfOp(Name("x"), Name("y"))): + assert True + case _: + assert False + + def test_right_of(self): + mod = parse_string_helper("x right of y ") + stmt = mod.body[0] + match stmt: + case Expr(RightOfOp(Name("x"), Name("y"))): + assert True + case _: + assert False + def test_intersects(self): mod = parse_string_helper("x intersects y ") stmt = mod.body[0] diff --git a/tests/syntax/test_recording.py b/tests/syntax/test_recording.py new file mode 100644 index 000000000..4c03cff60 --- /dev/null +++ b/tests/syntax/test_recording.py @@ -0,0 +1,110 @@ +"""Tests for advanced usages of the `record` statement.""" + +import numpy as np + +from tests.utils import compileScenic, sampleResult + +## Utilities + + +def checkRecordTo(tmp_path, period=None, delay=None, maxSteps=5): + """Helper for testing the `record ... to ...` statement. + + Returns the timesteps at which a value was recorded. + """ + + # Clear out the folder in case the helper is used multiple times in a test + for f in tmp_path.iterdir(): + f.unlink() + + every = f"every {period}" if period else "" + after = f"after {delay}" if delay else "" + scenario = compileScenic( + f""" + record -simulation().currentTime {every} {after} to "value_{{step}}.npy" + record -simulation().currentTime {every} {after} to "series.npz" + """, + params=dict(recordFolder=tmp_path), + ) + result = sampleResult(scenario, maxSteps=maxSteps) + assert result is not None + + recordedTimes = [] + for t in range(maxSteps + 1): + path = tmp_path / f"value_{t}.npy" + if path.exists(): + value = np.load(path) + assert float(value) == -t + recordedTimes.append(t) + + series = np.load(tmp_path / "series.npz") + assert np.array_equal(series["timesteps"], recordedTimes) + assert np.array_equal(series["values"], [-t for t in recordedTimes]) + + return recordedTimes + + +## Recording to files + + +def test_record_to(tmp_path): + times = checkRecordTo(tmp_path) + assert times == [0, 1, 2, 3, 4, 5] + + +def test_record_to_after(tmp_path): + times = checkRecordTo(tmp_path, delay="2 steps") + assert times == [2, 3, 4, 5] + + times = checkRecordTo(tmp_path, delay="3.5 seconds") + assert times == [3, 4, 5] + + times = checkRecordTo(tmp_path, delay="10 steps") + assert times == [] + + +def test_record_to_every(tmp_path): + times = checkRecordTo(tmp_path, period="2 steps") + assert times == [0, 2, 4] + + times = checkRecordTo(tmp_path, period="3.5 seconds") + assert times == [0, 3] + + times = checkRecordTo(tmp_path, period="10 steps") + assert times == [0] + + +def test_record_to_in_subscenario(tmp_path): + scenario = compileScenic( + """ + scenario Main(): + compose: + wait for 2 steps + do Sub(1) + wait for 2 steps + do Sub(2) + wait + scenario Sub(i): + record simulation().currentTime to "value_{step}.npy" + record simulation().currentTime to f"series{i}.npz" + terminate after 2 steps + """, + params=dict(recordFolder=tmp_path), + ) + result = sampleResult(scenario, maxSteps=10) + assert result is not None + + for t in range(11): + path = tmp_path / f"value_{t}.npy" + if 2 <= t <= 4 or 6 <= t <= 8: + value = np.load(path) + assert float(value) == t + else: + assert not path.exists(), t + + series1 = np.load(tmp_path / "series1.npz") + assert np.array_equal(series1["timesteps"], [2, 3, 4]) + assert np.array_equal(series1["values"], [2, 3, 4]) + series2 = np.load(tmp_path / "series2.npz") + assert np.array_equal(series2["timesteps"], [6, 7, 8]) + assert np.array_equal(series2["values"], [6, 7, 8]) diff --git a/tests/utils.py b/tests/utils.py index d8b284fcf..8da9fc189 100644 --- a/tests/utils.py +++ b/tests/utils.py @@ -185,11 +185,17 @@ def sampleTrajectory( ) -def sampleResult(scenario, maxIterations=1, maxSteps=1, maxScenes=1, timestep=1): +def sampleResult( + scenario, maxIterations=1, maxSteps=1, maxScenes=1, timestep=1, simulator=None +): for i in range(maxScenes): scene, iterations = generateChecked(scenario, maxIterations) result = sampleResultFromScene( - scene, maxIterations=maxIterations, maxSteps=maxSteps, timestep=timestep + scene, + maxIterations=maxIterations, + maxSteps=maxSteps, + timestep=timestep, + simulator=simulator, ) if result is not None: return result @@ -198,16 +204,21 @@ def sampleResult(scenario, maxIterations=1, maxSteps=1, maxScenes=1, timestep=1) ) -def sampleResultOnce(scenario, maxSteps=1, timestep=1): +def sampleResultOnce(scenario, maxSteps=1, timestep=1, simulator=None): scene = sampleScene(scenario) - sim = DummySimulator() + sim = DummySimulator() if simulator is None else simulator return sim.simulate(scene, maxSteps=maxSteps, maxIterations=1, timestep=timestep) def sampleResultFromScene( - scene, maxIterations=1, maxSteps=1, raiseGuardViolations=False, timestep=1 + scene, + maxIterations=1, + maxSteps=1, + raiseGuardViolations=False, + timestep=1, + simulator=None, ): - sim = DummySimulator() + sim = DummySimulator() if simulator is None else simulator simulation = sim.simulate( scene, maxSteps=maxSteps, diff --git a/tools/benchmarking/parallelization/benchmark_scene_parallel.py b/tools/benchmarking/parallelization/benchmark_scene_parallel.py new file mode 100644 index 000000000..27b2b64fb --- /dev/null +++ b/tools/benchmarking/parallelization/benchmark_scene_parallel.py @@ -0,0 +1,84 @@ +from pathlib import Path +import time +import warnings + +import scenic + +NUM_WORKERS = 8 +BENCHMARKS_BASE_PATH = (Path(__file__).resolve().parent / "benchmarks").resolve() +MAP_PATH = ( + Path(__file__).resolve().parent.parent.parent.parent + / "assets" + / "maps" + / "CARLA" + / "Town05.xodr" +).resolve() +MESH_BASE_PATH = ( + Path(__file__).resolve().parent.parent.parent.parent / "assets" / "meshes" +).resolve() + +BENCHMARKS = [ + ("adjacentOpposingPair.scenic", {"mode2D": True, "map": MAP_PATH}), + ("badlyParkedCarPullingIn.scenic", {"mode2D": True, "map": MAP_PATH}), + ("bypassing_03.scenic", {"mode2D": True, "map": MAP_PATH}), + ("city_intersection.scenic", {"meshBasePath": MESH_BASE_PATH}), + # ("enclosed_occluded.scenic", {}), + ("enclosed_visible.scenic", {}), + ("fully_occluded.scenic", {"meshBasePath": MESH_BASE_PATH}), + ("fully_visible.scenic", {"meshBasePath": MESH_BASE_PATH}), + ("narrowGoalNew.scenic", {"meshBasePath": MESH_BASE_PATH}), + ("narrowGoalOld.scenic", {"mode2D": True, "map": MAP_PATH}), + ("pedestrian_02.scenic", {"mode2D": True, "map": MAP_PATH}), + ("vacuum.scenic", {"numToys": 0, "meshBasePath": MESH_BASE_PATH}), + ("vacuum.scenic", {"numToys": 1, "meshBasePath": MESH_BASE_PATH}), + ("vacuum.scenic", {"numToys": 2, "meshBasePath": MESH_BASE_PATH}), + ("vacuum.scenic", {"numToys": 4, "meshBasePath": MESH_BASE_PATH}), + ("vacuum.scenic", {"numToys": 8, "meshBasePath": MESH_BASE_PATH}), + # ("vacuum.scenic", {"numToys": 16, "meshBasePath": MESH_BASE_PATH}), +] + +NUM_SAMPLES = 128 + + +def run_benchmark(path, params): + scenario = scenic.scenarioFromFile( + BENCHMARKS_BASE_PATH / path, params=params, mode2D=params.get("mode2D", False) + ) + for _ in range(NUM_SAMPLES): + scenario.generate(maxIterations=float("inf")) + + +def run_benchmark_parallel(path, params, *, numWorkers): + scenario = scenic.scenarioFromFile( + BENCHMARKS_BASE_PATH / path, params=params, mode2D=params.get("mode2D", False) + ) + scenario.generateBatch(NUM_SAMPLES, numWorkers=numWorkers) + + +if __name__ == "__main__": + # print("Base Performance (`generate`, numWorkers=0):") + # for benchmark in BENCHMARKS: + # with warnings.catch_warnings(): + # warnings.simplefilter("ignore") + # start = time.time() + # run_benchmark(*benchmark) + # trial_time = time.time() - start + # print(f"{trial_time: 7.2f} | {benchmark}") + # print() + # print("Base + Overhead Performance (`generateBatch`, numWorkers=1):") + # for benchmark in BENCHMARKS: + # with warnings.catch_warnings(): + # warnings.simplefilter("ignore") + # start = time.time() + # run_benchmark_parallel(*benchmark, numWorkers=1) + # trial_time = time.time() - start + # print(f"{trial_time: 7.2f} | {benchmark}") + # print() + print("Batch Performance (`generateBatch`, numWorkers=8):") + for benchmark in BENCHMARKS: + with warnings.catch_warnings(): + warnings.simplefilter("ignore") + start = time.time() + run_benchmark_parallel(*benchmark, numWorkers=8) + trial_time = time.time() - start + print(f"{trial_time: 7.2f} | {benchmark}") diff --git a/tools/benchmarking/parallelization/benchmark_sim_parallel.py b/tools/benchmarking/parallelization/benchmark_sim_parallel.py new file mode 100644 index 000000000..4d373b013 --- /dev/null +++ b/tools/benchmarking/parallelization/benchmark_sim_parallel.py @@ -0,0 +1,89 @@ +from pathlib import Path +import time +import warnings + +import scenic +from scenic.core.utils import SimulatorGroup +from scenic.simulators.metadrive.simulator import MetaDriveSimulator + +NUM_WORKERS = 8 +BENCHMARKS_BASE_PATH = (Path(__file__).resolve().parent / "benchmarks").resolve() +MAP_PATH = ( + Path(__file__).resolve().parent.parent.parent.parent + / "assets" + / "maps" + / "CARLA" + / "Town05.xodr" +).resolve() +SUMO_MAP_PATH = ( + Path(__file__).resolve().parent.parent.parent.parent + / "assets" + / "maps" + / "CARLA" + / "Town05.net.xml" +).resolve() +MESH_BASE_PATH = ( + Path(__file__).resolve().parent.parent.parent.parent / "assets" / "meshes" +).resolve() + +BENCHMARKS = [ + ("badlyParkedCarPullingIn.scenic", {"mode2D": True, "map": MAP_PATH}), + ("bypassing_03.scenic", {"mode2D": True, "map": MAP_PATH}), +] + +NUM_SAMPLES = 128 + + +def run_benchmark(path, params): + simulator = MetaDriveSimulator(sumo_map=SUMO_MAP_PATH, render=False, real_time=False) + scenario = scenic.scenarioFromFile( + BENCHMARKS_BASE_PATH / path, params=params, mode2D=params.get("mode2D", False) + ) + for _ in range(NUM_SAMPLES): + scene, _ = scenario.generate(maxIterations=float("inf")) + simulator.simulate(scene) + + +def run_benchmark_parallel(path, params, *, numWorkers): + scenario = scenic.scenarioFromFile( + BENCHMARKS_BASE_PATH / path, params=params, mode2D=params.get("mode2D", False) + ) + scene_stream = scenario.generateStream( + NUM_SAMPLES, numWorkers=numWorkers, serialized=True + ) + sim_group = SimulatorGroup( + numWorkers=numWorkers, + simulatorClass=MetaDriveSimulator, + simulatorParams={"sumo_map": SUMO_MAP_PATH, "render": False, "real_time": False}, + mute=False, + ) + sim_group.simulateBatch(scenario=scenario, scenes=scene_stream) + + +if __name__ == "__main__": + print("Base Performance (`generate`, numWorkers=0):") + for benchmark in BENCHMARKS: + with warnings.catch_warnings(): + warnings.simplefilter("ignore") + start = time.time() + run_benchmark(*benchmark) + trial_time = time.time() - start + print(f"{trial_time: 7.2f} | {benchmark}") + print() + print("Base + Overhead Performance (`generateBatch`, numWorkers=1):") + for benchmark in BENCHMARKS: + with warnings.catch_warnings(): + warnings.simplefilter("ignore") + start = time.time() + run_benchmark_parallel(*benchmark, numWorkers=1) + trial_time = time.time() - start + print(f"{trial_time: 7.2f} | {benchmark}") + print() + print("Batch Performance (`generateBatch`, numWorkers=8):") + for benchmark in BENCHMARKS: + with warnings.catch_warnings(): + warnings.simplefilter("ignore") + start = time.time() + run_benchmark_parallel(*benchmark, numWorkers=8) + trial_time = time.time() - start + print(f"{trial_time: 7.2f} | {benchmark}") diff --git a/tools/benchmarking/parallelization/benchmarks/adjacentOpposingPair.scenic b/tools/benchmarking/parallelization/benchmarks/adjacentOpposingPair.scenic new file mode 100644 index 000000000..055bc93fe --- /dev/null +++ b/tools/benchmarking/parallelization/benchmarks/adjacentOpposingPair.scenic @@ -0,0 +1,6 @@ +model scenic.simulators.carla.model + +ego = new Car with visibleDistance 20 +c2 = new Car visible +c3 = new Car at c2 offset by Range(-10, 1) @ 0 +require abs(relative heading of c3 from c2) >= 150 deg \ No newline at end of file diff --git a/tools/benchmarking/parallelization/benchmarks/badlyParkedCarPullingIn.scenic b/tools/benchmarking/parallelization/benchmarks/badlyParkedCarPullingIn.scenic new file mode 100644 index 000000000..ba566dc73 --- /dev/null +++ b/tools/benchmarking/parallelization/benchmarks/badlyParkedCarPullingIn.scenic @@ -0,0 +1,30 @@ +param time_step = 1.0/10 + +model scenic.simulators.metadrive.model + +behavior PullIntoRoad(): + while (distance from self to ego) > 15: + wait + do FollowLaneBehavior(laneToFollow=ego.lane) + +ego = new Car with behavior DriveAvoidingCollisions(avoidance_threshold=5) + +rightCurb = ego.laneGroup.curb +spot = new OrientedPoint on visible rightCurb +badAngle = Uniform(1.0, -1.0) * Range(10, 20) deg +parkedCar = new Car left of spot by 0.5, + facing badAngle relative to roadDirection, + with behavior PullIntoRoad + +require (distance to parkedCar) > 20 + +monitor StopAfterInteraction(): + for i in range(50): + wait + while ego.speed > 2: + wait + for i in range(50): + wait + terminate +require monitor StopAfterInteraction() +terminate after 15 seconds # in case ego never breaks diff --git a/tools/benchmarking/parallelization/benchmarks/bypassing_03.scenic b/tools/benchmarking/parallelization/benchmarks/bypassing_03.scenic new file mode 100644 index 000000000..647f90ed5 --- /dev/null +++ b/tools/benchmarking/parallelization/benchmarks/bypassing_03.scenic @@ -0,0 +1,107 @@ +""" +TITLE: Bypassing 03 +AUTHOR: Francis Indaheng, findaheng@berkeley.edu +DESCRIPTION: Ego vehicle performs a lane change to bypass a slow +adversary vehicle but cannot return to its original lane because +the adversary accelerates. Ego vehicle must then slow down to avoid +collision with leading vehicle in new lane. +SOURCE: NHSTA, #16 +""" + +################################# +# MAP AND MODEL # +################################# + +model scenic.simulators.metadrive.model + +################################# +# CONSTANTS # +################################# + +MODEL = 'vehicle.lincoln.mkz2017' + +param EGO_SPEED = VerifaiRange(7, 10) +param EGO_BRAKE = VerifaiRange(0.7, 1.0) + +param ADV_DIST = VerifaiRange(10, 15) +param ADV_INIT_SPEED = VerifaiRange(2, 4) +param ADV_END_SPEED = 2 * VerifaiRange(7, 10) +ADV_BUFFER_TIME = 5 + +LEAD_DIST = globalParameters.ADV_DIST + 10 +LEAD_SPEED = globalParameters.EGO_SPEED - 4 + +BYPASS_DIST = [15, 10] +SAFE_DIST = 15 +INIT_DIST = 50 +TERM_DIST = 70 +TERM_TIME = 10 + +################################# +# AGENT BEHAVIORS # +################################# + +behavior DecelerateBehavior(brake): + take SetBrakeAction(brake) + +behavior EgoBehavior(): + try: + do FollowLaneBehavior(target_speed=globalParameters.EGO_SPEED) + interrupt when (distance to adversary) < BYPASS_DIST[0]: + fasterLaneSec = self.laneSection.fasterLane + do LaneChangeBehavior( + laneSectionToSwitch=fasterLaneSec, + target_speed=globalParameters.EGO_SPEED) + try: + do FollowLaneBehavior( + target_speed=globalParameters.EGO_SPEED, + laneToFollow=fasterLaneSec.lane) \ + until (distance to adversary) > BYPASS_DIST[1] + interrupt when (distance to lead) < SAFE_DIST: + try: + do DecelerateBehavior(globalParameters.EGO_BRAKE) + interrupt when (distance to lead) > SAFE_DIST: + do FollowLaneBehavior(target_speed=LEAD_SPEED) for TERM_TIME seconds + terminate + +behavior AdversaryBehavior(): + do FollowLaneBehavior(target_speed=globalParameters.ADV_INIT_SPEED) \ + until self.lane is not ego.lane + do FollowLaneBehavior(target_speed=globalParameters.ADV_END_SPEED) + +behavior LeadBehavior(): + fasterLaneSec = self.laneSection.fasterLane + do LaneChangeBehavior( + laneSectionToSwitch=fasterLaneSec, + target_speed=LEAD_SPEED) + do FollowLaneBehavior(target_speed=LEAD_SPEED) + +################################# +# SPATIAL RELATIONS # +################################# + +initLane = Uniform(*network.lanes) +egoSpawnPt = new OrientedPoint in initLane.centerline + +################################# +# SCENARIO SPECIFICATION # +################################# + +ego = new Car at egoSpawnPt, + with blueprint MODEL, + with behavior EgoBehavior() + +adversary = new Car following roadDirection for globalParameters.ADV_DIST, + with blueprint MODEL, + with behavior AdversaryBehavior() + +lead = new Car following roadDirection for LEAD_DIST, + with blueprint MODEL, + with behavior LeadBehavior() + +require (distance to intersection) > INIT_DIST +require (distance from adversary to intersection) > INIT_DIST +require (distance from lead to intersection) > INIT_DIST +require always (adversary.laneSection._fasterLane is not None) +terminate when (distance to egoSpawnPt) > TERM_DIST +terminate after 60 seconds diff --git a/tools/benchmarking/parallelization/benchmarks/city_intersection.scenic b/tools/benchmarking/parallelization/benchmarks/city_intersection.scenic new file mode 100644 index 000000000..6afe51efc --- /dev/null +++ b/tools/benchmarking/parallelization/benchmarks/city_intersection.scenic @@ -0,0 +1,131 @@ +""" +Generate a city intersection driving scenario, an intersection +of two 2-lane one way roads in a city. +""" + +model scenic.simulators.webots.model + +import shapely +import time +import shutil +import os +from pathlib import Path + +class EgoCar(WebotsObject): + webotsName: "EGO" + shape: MeshShape.fromFile(globalParameters.meshBasePath / "bmwx5_hull.obj.bz2", initial_rotation=(90 deg, 0, 0)) + positionOffset: Vector(-1.43580750, 0, -0.557354985).rotatedBy(Orientation.fromEuler(*self.orientationOffset)) + cameraOffset: Vector(-1.43580750, 0, -0.557354985) + Vector(1.72, 0, 1.4) + orientationOffset: (90 deg, 0, 0) + viewAngles: (1.5, 60 deg) + visibleDistance: 100 + rayDensity: 10 + +class Car(EgoCar): + webotsName: "CAR" + +class CommercialBuilding(WebotsObject): + webotsType: "BUILDING_COMMERCIAL" + width: 22 + length: 22 + height: 100 + yaw: Uniform(1, 2, 3) * 90 deg + +class ResidentialBuilding(WebotsObject): + webotsType: "BUILDING_RESIDENTIAL" + width: 14.275 + length: 57.4 + height: 40 + yaw: 90 deg + +class GlassBuilding(WebotsObject): + webotsType: "BUILDING_GLASS" + width: 14.1 + length: 8.1 + height: 112 + yaw: Uniform(1, 2, 3) * 90 deg + +class LogImageAction(Action): + def __init__(self, visible: bool, path: str, count: int): + self.visible = visible + self.path = path + self.count = count + + def applyTo(self, obj, sim): + print("Other Car Visible:", self.visible) + + target_path = self.path + "/" + target_path += "visible" if self.visible else "invisible" + + if not os.path.exists(target_path): + os.makedirs(target_path) + + target_path += "/" + str(self.count) + ".jpeg" + + print("IMG Path:", target_path) + + # Wait for other controller to write image + time.sleep(0.001) + attempts = 0 + while not os.path.exists(localPath("images/live_img.jpeg")): + print("Waiting for image...") + attempts += 1 + time.sleep(0.001) + + if attempts > 10: + print("Could not move image...") + return + + shutil.move(localPath("images/live_img.jpeg"), target_path) + +behavior LogCamera(path): + count = 0 + while True: + visible = ego can see car + take LogImageAction(visible, path, count) + count += 1 + +# Create a region that represents both lanes of the crossing road. +crossing_road_lane = RectangularRegion((0,0,0.02), 0, 160, 5) + +car = new Car facing 90 deg, on crossing_road_lane, with regionContainedIn crossing_road_lane +require car.x > 10 + +# Create a region that represents both lanes of the bottom road. +bottom_road_lane = RectangularRegion((0,-55,0.02), 0, 5, 80) + +# Place the ego car in one of the lanes, and ensure it is fully contained. +ego = new EgoCar on bottom_road_lane, with regionContainedIn bottom_road_lane, with behavior LogCamera(localPath(f"images/{time.time_ns()}")) + +# Create a region composed of all 4 quadrants around the road +top_right_quadrant = RectangularRegion(56@56, 0, 100, 100) +top_left_quadrant = RectangularRegion(-56@56, 0, 100, 100) +bottom_right_quadrant = RectangularRegion(56@-56, 0, 100, 100) +bottom_left_quadrant = RectangularRegion(-56@-56, 0, 100, 100) + +building_region = top_right_quadrant.union(top_left_quadrant) + +# Add buildings, some randomly, some designed to block visibility of the center road +for _ in range(1): + new CommercialBuilding in building_region, with regionContainedIn building_region + +for _ in range(2): + new ResidentialBuilding in building_region, with regionContainedIn building_region + +for _ in range(2): + new GlassBuilding in building_region, with regionContainedIn building_region + +new ResidentialBuilding at (-36, -21, 0) +new CommercialBuilding at (18 + Range(-1,1), -20 + Range(-1,1), 0), facing Range(-5,5) deg +new CommercialBuilding at (50 + Range(-1,1), -22 + Range(-1,1), 0), facing Range(-5,5) deg + +# Terminate the simulation after the ego has passed through the intersection or a timeout is reached +terminate when ego.position.y > 0 +terminate after 60 seconds + +# Require that the ego can eventually see the crossing car, but not until it gets close. +require eventually (ego can see car) +require (not ego can see car) until (distance from ego to car < 75) + +# Require that the cars do not crash +require always distance to car > 2 diff --git a/tools/benchmarking/parallelization/benchmarks/enclosed_occluded.scenic b/tools/benchmarking/parallelization/benchmarks/enclosed_occluded.scenic new file mode 100644 index 000000000..0856487cf --- /dev/null +++ b/tools/benchmarking/parallelization/benchmarks/enclosed_occluded.scenic @@ -0,0 +1,20 @@ +""" Tests visibility calculation time on an object completely enclosed in two spheres, +trying to view the outside sphere. +""" + +from scipy.spatial.transform import Rotation +from pathlib import Path + +workspace = Workspace(everywhere) + +ego = new Object facing Orientation(Rotation.random()), at (Range(-0.1,0.1),Range(-0.1,0.1),Range(-0.1,0.1)) + +def get_shape(hole_size): + return MeshShape( + SpheroidRegion(dimensions=(1,1,1)).difference( + SpheroidRegion(dimensions=(0.9,0.9,0.9))).difference( + BoxRegion(dimensions=(hole_size,0.5,hole_size), position=(0,0.5,0))).mesh + ) + +occluding_sphere = new Object at (0,0,0), with shape get_shape(0.1), with width 3, with length 3, with height 3 +target_sphere = new Object at (0,0,0), with shape get_shape(0.3), with width 5, with length 5, with height 5, not visible diff --git a/tools/benchmarking/parallelization/benchmarks/enclosed_visible.scenic b/tools/benchmarking/parallelization/benchmarks/enclosed_visible.scenic new file mode 100644 index 000000000..52cc2b4d9 --- /dev/null +++ b/tools/benchmarking/parallelization/benchmarks/enclosed_visible.scenic @@ -0,0 +1,22 @@ +""" Tests visibility calculation time on an object almost completely enclosed in two spheres, +trying to view the outside sphere. +""" + +from scipy.spatial.transform import Rotation +from pathlib import Path + +workspace = Workspace(everywhere) + +ego = new Object facing Orientation(Rotation.random()), at (Range(-0.1,0.1),Range(-0.1,0.1),Range(-0.1,0.1)) + +hollow_sphere_shape = MeshShape( + SpheroidRegion(dimensions=(5,5,5)).difference(SpheroidRegion(dimensions=(4.8,4.8,4.8))).mesh + ) + +hollow_sphere_shape_with_hole = MeshShape( + SpheroidRegion(dimensions=(5.2,5.2,5.2)).difference(SpheroidRegion(dimensions=(5.01,5.01,5.01))).difference( + BoxRegion(dimensions=(0.1,0.1,1), position=(0,0,2.5))).mesh + ) + +occluding_sphere = new Object at (0,0,0), with shape hollow_sphere_shape_with_hole +target_sphere = new Object at (0,0,0), with shape hollow_sphere_shape, visible diff --git a/tools/benchmarking/parallelization/benchmarks/fully_occluded.scenic b/tools/benchmarking/parallelization/benchmarks/fully_occluded.scenic new file mode 100644 index 000000000..b61ade734 --- /dev/null +++ b/tools/benchmarking/parallelization/benchmarks/fully_occluded.scenic @@ -0,0 +1,15 @@ +""" Tests visibility calculation time on a complex mesh, completely occluded by another complex mesh""" + +from pathlib import Path + +workspace = Workspace(everywhere) + +ego = new Object + +chair_shape = MeshShape.fromFile(path=globalParameters.meshBasePath / "chair.obj.bz2", initial_rotation=(0,90 deg,0)) + +obscuring_chair = new Object with shape chair_shape, at (0,5,0), + with pitch -90 deg, with width 5, with length 5, with height 5 + +target_chair = new Object with shape chair_shape, at (0,10,0), + with width 3, with length 3, with height 3, not visible diff --git a/tools/benchmarking/parallelization/benchmarks/fully_visible.scenic b/tools/benchmarking/parallelization/benchmarks/fully_visible.scenic new file mode 100644 index 000000000..59d2935f9 --- /dev/null +++ b/tools/benchmarking/parallelization/benchmarks/fully_visible.scenic @@ -0,0 +1,12 @@ +""" Tests visibility calculation time on a complex mesh that doesn't contain its center """ + +from pathlib import Path + +workspace = Workspace(everywhere) + +ego = new Object + +chair_shape = MeshShape.fromFile(path=globalParameters.meshBasePath / "chair.obj.bz2", initial_rotation=(0,90 deg,0)) + +target_chair = new Object with shape chair_shape, at (0,10,0), + with width 3, with length 3, with height 3, visible diff --git a/tools/benchmarking/parallelization/benchmarks/narrowGoalNew.scenic b/tools/benchmarking/parallelization/benchmarks/narrowGoalNew.scenic new file mode 100644 index 000000000..2199d91e0 --- /dev/null +++ b/tools/benchmarking/parallelization/benchmarks/narrowGoalNew.scenic @@ -0,0 +1,102 @@ +model scenic.simulators.webots.model + +from pathlib import Path + +# Set up workspace +width = 10 +length = 10 +workspace = Workspace(RectangularRegion(0 @ 0, 0, width, length)) + +# types of objects + +class MarsGround(Ground): + width: width + length: length + gridSize: 20 + +class MarsHill(Hill): + position: new Point in workspace + width: Range(1,2) + length: Range(1,2) + height: Range(0.1, 0.3) + spread: Range(0.2, 0.3) + regionContainedIn: everywhere + +class Goal(WebotsObject): + """Flag indicating the goal location.""" + width: 0.1 + length: 0.1 + webotsType: 'GOAL' + +class Rover(WebotsObject): + """Mars rover.""" + width: 0.5 + length: 0.7 + height: 0.4 + webotsType: 'ROVER' + rotationOffset: (90 deg, 0, 0) + +class Debris(WebotsObject): + """Abstract class for debris scattered randomly in the workspace.""" + # Recess things into the ground slightly by default + baseOffset: (0, 0, -self.height/3) + +class BigRock(Debris): + """Large rock.""" + shape: MeshShape.fromFile(globalParameters.meshBasePath / "webots_rock_large.obj.bz2") + yaw: Range(0, 360 deg) + webotsType: 'ROCK_BIG' + positionOffset: Vector(0,0, -self.height/2) + +class Rock(Debris): + """Small rock.""" + shape: MeshShape.fromFile(globalParameters.meshBasePath / "webots_rock_small.obj.bz2") + yaw: Range(0, 360 deg) + webotsType: 'ROCK_SMALL' + positionOffset: Vector(0,0, -self.height/2) + +class Pipe(Debris): + """Pipe with variable length.""" + width: 0.2 + length: Range(0.5, 1.5) + height: self.width + shape: CylinderShape(initial_rotation=(90 deg, 0, 90 deg)) + yaw: Range(0, 360 deg) + webotsType: 'PIPE' + rotationOffset: (90 deg, 0, 90 deg) + + def startDynamicSimulation(self): + # Apply variable length + self.webotsObject.getField('height').setSFFloat(self.length) + +# Ground with random gaussian hills +ground = new MarsGround on (0,0,0), with terrain [new MarsHill for _ in range(60)] + +# Ego and goal on ground +ego = new Rover at (0, -3), on ground, with controller 'sojourner' +goal = new Goal at (Range(-2, 2), Range(2, 3)), on ground, facing (0,0,0) + +# Bottleneck made of two pipes with a rock in between +bottleneck = new OrientedPoint at ego offset by Range(-1.5, 1.5) @ Range(0.5, 1.5), facing Range(-30, 30) deg +require abs((angle to goal) - (angle to bottleneck)) <= 10 deg +new BigRock at bottleneck, on ground + +gap = 1.2 * ego.width +halfGap = gap / 2 + +leftEdge = new OrientedPoint left of bottleneck by halfGap, + facing Range(60, 120) deg relative to bottleneck.heading +rightEdge = new OrientedPoint right of bottleneck by halfGap, + facing Range(-120, -60) deg relative to bottleneck.heading + +new Pipe ahead of leftEdge, with length Range(1, 2), on ground, facing leftEdge, with parentOrientation 0 +new Pipe ahead of rightEdge, with length Range(1, 2), on ground, facing rightEdge, with parentOrientation 0 + +# Other junk because why not? + +new Pipe on ground, with parentOrientation 0 +new BigRock beyond bottleneck by Range(0.25, 0.75) @ Range(0.75, 1), on ground +new BigRock beyond bottleneck by Range(-0.75, -0.25) @ Range(0.75, 1), on ground +new Rock on ground +new Rock on ground +new Rock on ground diff --git a/tools/benchmarking/parallelization/benchmarks/narrowGoalOld.scenic b/tools/benchmarking/parallelization/benchmarks/narrowGoalOld.scenic new file mode 100644 index 000000000..aec4524bd --- /dev/null +++ b/tools/benchmarking/parallelization/benchmarks/narrowGoalOld.scenic @@ -0,0 +1,83 @@ +from scenic.simulators.webots.model import WebotsObject + +# Set up workspace +width = 5 +length = 5 +workspace = Workspace(RectangularRegion(0 @ 0, 0, width, length)) + +# types of objects + +class Goal(WebotsObject): + """Flag indicating the goal location.""" + width: 0.3 + length: 0.3 + webotsType: 'GOAL' + +class Rover(WebotsObject): + """Mars rover.""" + width: 0.5 + length: 0.7 + webotsType: 'ROVER' + rotationOffset: 90 deg + +class Debris(WebotsObject): + """Abstract class for debris scattered randomly in the workspace.""" + position: new Point in workspace + heading: Range(0, 360) deg + +class BigRock(Debris): + """Large rock.""" + width: 0.17 + length: 0.17 + webotsType: 'ROCK_BIG' + +class Rock(Debris): + """Small rock.""" + width: 0.10 + length: 0.10 + webotsType: 'ROCK_SMALL' + +class Pipe(Debris): + """Pipe with variable length.""" + width: 0.2 + length: Range(0.5, 1.5) + webotsType: 'PIPE' + + def startDynamicSimulation(self): + # Apply variable length + self.webotsObject.getField('height').setSFFloat(self.length) + # Apply 3D rotation to make pipes lie flat on surface + rotation = [cos(self.heading), sin(self.heading), 0, 90 deg] + self.webotsObject.getField('rotation').setSFRotation(rotation) + +ego = new Rover at 0 @ -2 + +goal = new Goal at Range(-2, 2) @ Range(2, 2.5) + +# Bottleneck made of two pipes with a rock in between + +gap = 1.2 * ego.width +halfGap = gap / 2 + +bottleneck = new OrientedPoint offset by Range(-1.5, 1.5) @ Range(0.5, 1.5), facing Range(-30, 30) deg + +require abs((angle to goal) - (angle to bottleneck)) <= 10 deg + +new BigRock at bottleneck + +leftEdge = new OrientedPoint at bottleneck offset by -halfGap @ 0, + facing Range(60, 120) deg relative to bottleneck.heading +rightEdge = new OrientedPoint at bottleneck offset by halfGap @ 0, + facing Range(-120, -60) deg relative to bottleneck.heading + +new Pipe ahead of leftEdge, with length Range(1, 2) +new Pipe ahead of rightEdge, with length Range(1, 2) + +# Other junk because why not? + +new Pipe +new BigRock beyond bottleneck by Range(-0.5, 0.5) @ Range(0.5, 1) +new BigRock beyond bottleneck by Range(-0.5, 0.5) @ Range(0.5, 1) +new Rock +new Rock +new Rock \ No newline at end of file diff --git a/tools/benchmarking/parallelization/benchmarks/pedestrian_02.scenic b/tools/benchmarking/parallelization/benchmarks/pedestrian_02.scenic new file mode 100644 index 000000000..5d2fc408e --- /dev/null +++ b/tools/benchmarking/parallelization/benchmarks/pedestrian_02.scenic @@ -0,0 +1,89 @@ +""" +TITLE: Pedestrian 02 +AUTHOR: Francis Indaheng, findaheng@berkeley.edu +DESCRIPTION: Both ego and adversary vehicles must suddenly stop to avoid +collision when pedestrian crosses the road unexpectedly. +SOURCE: Carla Challenge, #03 +""" + +################################# +# MAP AND MODEL # +################################# + +model scenic.simulators.carla.model + +################################# +# CONSTANTS # +################################# + +MODEL = 'vehicle.lincoln.mkz2017' + +param EGO_INIT_DIST = VerifaiRange(-30, -20) +param EGO_SPEED = VerifaiRange(7, 10) +EGO_BRAKE = 1.0 + +param ADV_INIT_DIST = VerifaiRange(40, 50) +param ADV_SPEED = VerifaiRange(7, 10) +ADV_BRAKE = 1.0 + +PED_MIN_SPEED = 1.0 +PED_THRESHOLD = 20 + +param SAFETY_DIST = VerifaiRange(10, 15) +BUFFER_DIST = 75 +CRASH_DIST = 5 +TERM_DIST = 50 + +################################# +# AGENT BEHAVIORS # +################################# + +behavior EgoBehavior(): + try: + do FollowLaneBehavior(target_speed=globalParameters.EGO_SPEED) + interrupt when withinDistanceToObjsInLane(self, globalParameters.SAFETY_DIST) and (ped in network.drivableRegion): + take SetBrakeAction(EGO_BRAKE) + interrupt when withinDistanceToAnyObjs(self, CRASH_DIST): + terminate + +behavior AdvBehavior(): + try: + do FollowLaneBehavior(target_speed=globalParameters.ADV_SPEED) + interrupt when (withinDistanceToObjsInLane(self, globalParameters.SAFETY_DIST) or (distance from adv to ped) < 10) and (ped in network.drivableRegion): + take SetBrakeAction(ADV_BRAKE) + interrupt when withinDistanceToAnyObjs(self, CRASH_DIST): + terminate + +################################# +# SPATIAL RELATIONS # +################################# + +road = Uniform(*filter(lambda r: len(r.forwardLanes.lanes) == len(r.backwardLanes.lanes) == 1, network.roads)) +egoLane = Uniform(road.forwardLanes.lanes)[0] +spawnPt = new OrientedPoint on egoLane.centerline +advSpawnPt = new OrientedPoint following roadDirection from spawnPt for globalParameters.ADV_INIT_DIST + +################################# +# SCENARIO SPECIFICATION # +################################# + +ego = new Car following roadDirection from spawnPt for globalParameters.EGO_INIT_DIST, + with blueprint MODEL, + with behavior EgoBehavior() + +ped = new Pedestrian right of spawnPt by 3, + with heading 90 deg relative to spawnPt.heading, + with regionContainedIn None, + with behavior CrossingBehavior(ego, PED_MIN_SPEED, PED_THRESHOLD) + +adv = new Car left of advSpawnPt by 3, + with blueprint MODEL, + with heading 180 deg relative to spawnPt.heading, + with behavior AdvBehavior() + +require (distance from spawnPt to intersection) > BUFFER_DIST +require always (ego.laneSection._slowerLane is None) +require always (ego.laneSection._fasterLane is None) +require always (adv.laneSection._slowerLane is None) +require always (adv.laneSection._fasterLane is None) +terminate when (distance to spawnPt) > TERM_DIST diff --git a/tools/benchmarking/parallelization/benchmarks/vacuum.scenic b/tools/benchmarking/parallelization/benchmarks/vacuum.scenic new file mode 100644 index 000000000..0b11fe6db --- /dev/null +++ b/tools/benchmarking/parallelization/benchmarks/vacuum.scenic @@ -0,0 +1,149 @@ +""" +Generate a room for the i-roomba create vacuum +""" +model scenic.simulators.webots.model + +import numpy as np +import trimesh +import random +from pathlib import Path + +param numToys = 0 +param duration = 10 + +## Class Definitions ## + +class Vacuum(WebotsObject): + webotsName: "IROBOT_CREATE" + shape: CylinderShape() + width: 0.335 + length: 0.335 + height: 0.07 + customData: str(random.getrandbits(32)) # Random seed for robot controller + +# Floor uses builtin Webots floor to keep Vacuum Sensors from breaking +# Not actually linked to WebotsObject because Webots floor is 2D +class Floor(Object): + width: 5 + length: 5 + height: 0.01 + position: (0,0,-0.005) + +class Wall(WebotsObject): + webotsAdhoc: {'physics': False} + width: 5 + length: 0.04 + height: 0.5 + +class DiningTable(WebotsObject): + webotsAdhoc: {'physics': True} + shape: MeshShape.fromFile(globalParameters.meshBasePath / "dining_table.obj.bz2") + width: Range(0.7, 1.5) + length: Range(0.7, 1.5) + height: 0.75 + density: 670 # Density of solid birch + +class DiningChair(WebotsObject): + webotsAdhoc: {'physics': True} + shape: MeshShape.fromFile(globalParameters.meshBasePath / "dining_chair.obj.bz2", initial_rotation=(180 deg, 0, 0)) + width: 0.4 + length: 0.4 + height: 1 + density: 670 # Density of solid birch + positionStdDev: (0.05, 0.05 ,0) + orientationStdDev: (10 deg, 0, 0) + +class Couch(WebotsObject): + webotsAdhoc: {'physics': False} + shape: MeshShape.fromFile(globalParameters.meshBasePath / "couch.obj.bz2", initial_rotation=(-90 deg, 0, 0)) + width: 2 + length: 0.75 + height: 0.75 + positionStdDev: (0.05, 0.5 ,0) + orientationStdDev: (5 deg, 0, 0) + +class CoffeeTable(WebotsObject): + webotsAdhoc: {'physics': False} + shape: MeshShape.fromFile(globalParameters.meshBasePath / "coffee_table.obj.bz2") + width: 1.5 + length: 0.5 + height: 0.4 + positionStdDev: (0.05, 0.05 ,0) + orientationStdDev: (5 deg, 0, 0) + +class Toy(WebotsObject): + webotsAdhoc: {'physics': True} + shape: Uniform(BoxShape(), CylinderShape(), ConeShape(), SpheroidShape()) + width: 0.1 + length: 0.1 + height: 0.1 + density: 100 + +class BlockToy(Toy): + shape: BoxShape() + +## Scene Layout ## + +# Create room region and set it as the workspace +room_region = RectangularRegion(0 @ 0, 0, 5.09, 5.09) +workspace = Workspace(room_region) + +# Create floor and walls +floor = new Floor +wall_offset = floor.width/2 + 0.04/2 + 1e-4 +right_wall = new Wall at (wall_offset, 0, 0.25), facing toward floor +left_wall = new Wall at (-wall_offset, 0, 0.25), facing toward floor +front_wall = new Wall at (0, wall_offset, 0.25), facing toward floor +back_wall = new Wall at (0, -wall_offset, 0.25), facing toward floor + +# Place vacuum on floor +ego = new Vacuum on floor + +# Create a "safe zone" around the vacuum so that it does not start stuck +safe_zone = CircularRegion(ego.position, radius=1) + +# Create a dining room region where we will place dining room furniture +dining_room_region = RectangularRegion(1.25 @ 0, 0, 2.5, 5).difference(safe_zone) + +# Place a table with 3 chairs around it, and one knocked over on the floor +dining_table = new DiningTable contained in dining_room_region, on floor, + facing Range(0, 360 deg) + +chair_1 = new DiningChair behind dining_table by -0.1, on floor, + facing toward dining_table, with regionContainedIn dining_room_region +chair_2 = new DiningChair ahead of dining_table by -0.1, on floor, + facing toward dining_table, with regionContainedIn dining_room_region +chair_3 = new DiningChair left of dining_table by -0.1, on floor, + facing toward dining_table, with regionContainedIn dining_room_region + +fallen_orientation = Uniform((0, -90 deg, 0), (0, 90 deg, 0), (0, 0, -90 deg), (0, 0, 90 deg)) + +chair_4 = new DiningChair contained in dining_room_region, facing fallen_orientation, + on floor, with baseOffset(0,0,-0.2) + +# Add some noise to the positions and yaw of the chairs around the table +mutate chair_1, chair_2, chair_3 + +# Create a living room region where we will place living room furniture +living_room_region = RectangularRegion(-1.25 @ 0, 0, 2.5, 5).difference(safe_zone) + +couch = new Couch ahead of left_wall by 0.335, + on floor, facing away from left_wall + +coffee_table = new CoffeeTable ahead of couch by 0.336, + on floor, facing away from couch + +# Add some noise to the positions of the couch and coffee table +mutate couch, coffee_table + +toy_stack = new BlockToy on floor +toy_stack = new BlockToy on toy_stack +toy_stack = new BlockToy on toy_stack + +# Spawn some toys +for _ in range(globalParameters.numToys): + new Toy on floor + +## Simulation Setup ## +terminate after globalParameters.duration * 60 seconds +record (ego.x, ego.y) as VacuumPosition