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..7f33c12d2 100644 --- a/docs/api.rst +++ b/docs/api.rst @@ -173,6 +173,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/reference/operators.rst b/docs/reference/operators.rst index 4a39539e9..0273979b6 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: @@ -75,6 +81,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/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 dc667ba7a..6f6cdf907 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -60,6 +60,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 +71,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/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/object_types.py b/src/scenic/core/object_types.py index 11ff01004..1d648252d 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 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..97469f479 100644 --- a/src/scenic/core/simulators.py +++ b/src/scenic/core/simulators.py @@ -489,6 +489,8 @@ def _run(self, dynamicScenario, maxSteps): # 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): @@ -811,7 +813,19 @@ def currentState(self): The default implementation returns a tuple of the positions of all objects. """ - return tuple(obj.position for obj in self.objects) + + class SimulationState(tuple): + def __new__(cls, positions, orientations): + return super().__new__(cls, positions) + + def __init__(self, positions, orientations): + self.positions = positions + self.orientations = orientations + + positions = tuple(obj.position for obj in self.objects) + orientation = tuple(obj.orientation for obj in self.objects) + + return SimulationState(positions, orientation) @property def currentRealTime(self): diff --git a/src/scenic/core/vectors.py b/src/scenic/core/vectors.py index c4ae11f48..83c640cfe 100644 --- a/src/scenic/core/vectors.py +++ b/src/scenic/core/vectors.py @@ -41,6 +41,7 @@ canCoerceType, coerceToFloat, toOrientation, + toVector, ) from scenic.core.utils import argsToString, cached_property @@ -702,6 +703,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 +739,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 +760,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/domains/driving/actions.py b/src/scenic/domains/driving/actions.py index 7f99cce1a..d8304ea9c 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. @@ -106,6 +109,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 +254,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.scenic b/src/scenic/domains/driving/behaviors.scenic index 173500190..5db4794a6 100644 --- a/src/scenic/domains/driving/behaviors.scenic +++ b/src/scenic/domains/driving/behaviors.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 LineString, MultiPoint, Point as ShapelyPoint + +from scenic.core.regions import toPolygon 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) behavior ConstantThrottleBehavior(x): while True: @@ -39,9 +43,56 @@ behavior WalkForwardBehavior(): 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 +101,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 + assert self.longitudinalController is not None + assert self.lateralController is not None - 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] - - # 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 toPolygon(self.polyline).project(ShapelyPoint(*pos), normalized=True)*self.duration - do TurnBehavior(trajectory = current_centerline) + def getTimedDistance(self, timeA, timeB): + return shapely.ops.substring(toPolygon(self.polyline), timeA/self.duration, timeB/self.duration, normalized=True).length + def __getitem__(self, time): + pt = toPolygon(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 ## + +def concatenateCenterlines(centerlines=[]): + return PolylineRegion.unionAll(centerlines) -behavior FollowTrajectoryBehavior(target_speed = 10, trajectory = None, turn_speed=None): +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 +251,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 +264,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/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/simulator.py b/src/scenic/simulators/metadrive/simulator.py index 76607d5bc..075134f55 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 diff --git a/src/scenic/syntax/__init__.py b/src/scenic/syntax/__init__.py index 9ea3b509d..8c9041b80 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(getParserHash()) + return result -if not _parserPath.exists(): +def getParserHash(): + with open(_parserPath, "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 == getParserHash() + + +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..56eca6a1f 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 @@ -650,3 +655,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..4e03ce67b 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) @@ -1683,9 +1683,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=( [] @@ -1850,3 +1861,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..31377a6de 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,8 +1850,11 @@ 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 diff --git a/src/scenic/syntax/veneer.py b/src/scenic/syntax/veneer.py index b79745030..d171dbeb3 100644 --- a/src/scenic/syntax/veneer.py +++ b/src/scenic/syntax/veneer.py @@ -65,7 +65,8 @@ "BottomBackLeft", "BottomBackRight", "RelativeHeading", - "ApparentHeading", + "ApparentHeadingOf", + "ApparentHeadingTo", "RelativePosition", "DistanceFrom", "DistancePast", @@ -84,6 +85,10 @@ "Implies", "VisibleFromOp", "NotVisibleFromOp", + "AheadOfOp", + "BehindOp", + "LeftOfOp", + "RightOfOp", # Primitive types "Vector", "Orientation", @@ -249,6 +254,7 @@ from contextlib import contextmanager import functools import importlib +import math import numbers from pathlib import Path import sys @@ -1275,7 +1281,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 +1290,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. @@ -1387,6 +1406,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_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_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/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..3e0d520d5 --- /dev/null +++ b/tests/syntax/helper_terminate.scenic @@ -0,0 +1 @@ +terminate after 1 seconds diff --git a/tests/syntax/test_compiler.py b/tests/syntax/test_compiler.py index 597b85b55..d4abc5956 100644 --- a/tests/syntax/test_compiler.py +++ b/tests/syntax/test_compiler.py @@ -2075,18 +2075,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("ApparentHeading"), [Name("X")]): + case Call(Name("ApparentHeadingOf"), [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_of_op_base(self): + node, _ = compileScenicAST(ApparentHeadingOfOp(Name("X"), Name("Y"))) match node: - case Call(Name("ApparentHeading"), [Name("X")], [keyword("Y", Name("Y"))]): + 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("ApparentHeadingTo"), [Name("X")]): + assert True + case _: + assert False + + def test_apparent_heading_to_op_base(self): + node, _ = compileScenicAST(ApparentHeadingToOp(Name("X"), Name("Y"))) + match node: + case Call(Name("ApparentHeadingTo"), [Name("X")], [keyword("Y", Name("Y"))]): assert True case _: assert False @@ -2290,6 +2306,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..3a7bd3c43 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 @@ -770,19 +789,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( """ diff --git a/tests/syntax/test_imports.py b/tests/syntax/test_imports.py index c5b3ca0a5..a2ddbf666 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,50 @@ 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" + """ + ) + + result = sampleResult(scenario, maxSteps=5) + assert "foo" in result.records + assert result.records["foo"] == [(0, 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)] + + def test_inherit_constructors(runLocally): with runLocally(): scenario = compileScenic("from helper import Caerbannog\n" "ego = new Caerbannog") diff --git a/tests/syntax/test_operators.py b/tests/syntax/test_operators.py index 7c405c789..1817b0dc6 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( @@ -602,6 +634,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..1394778c0 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()))), ), ], ) @@ -3005,6 +3097,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]