def __init__(self, lgsvl_scene, address='localhost', port=8181, alwaysReload=False):
super().__init__()
verbosePrint('Connecting to LGSVL Simulator...')
self.client = lgsvl.Simulator(address=address, port=port)
if alwaysReload or self.client.current_scene != lgsvl_scene:
self.client.load(scene=lgsvl_scene)
verbosePrint('Map loaded in simulator.')
def __init__(self, imagePath, Ax, Ay, Bx, By):
self.Ax, self.Ay = Ax, Ay
self.Bx, self.By = Bx, By
if imagePath != None:
startTime = time.time()
# open image
image = PIL.Image.open(imagePath)
self.sizeX, self.sizeY = image.size
# create version of image for display
de = img_modf.get_edges(image).convert('RGB')
self.displayImage = cv2.cvtColor(numpy.array(de),
cv2.COLOR_RGB2BGR)
# detect edges of roads
ed = center_detection.compute_midpoints(img_data=image,
kernelsize=5)
self.edgeData = {
self.gridToScenicCoords((x, y)): datum
for (y, x), datum in ed.items()
}
self.orderedCurbPoints = list(self.edgeData.keys())
# build k-D tree
self.edgeTree = scipy.spatial.cKDTree(self.orderedCurbPoints)
# identify points on roads
self.roadArray = numpy.array(
img_modf.convert_black_white(img_data=image).convert('L'),
dtype=int)
totalTime = time.time() - startTime
verbosePrint(
f'Created GTA map from image in {totalTime:.2f} seconds.')
def __init__(self, carla_map, map_path, address='127.0.0.1', port=2000, timeout=10,
render=True, record='', timestep=0.1):
super().__init__()
verbosePrint('Connecting to CARLA...')
self.client = carla.Client(address, port)
self.client.set_timeout(timeout) # limits networking operations (seconds)
if carla_map is not None:
self.world = self.client.load_world(carla_map)
else:
if map_path.endswith('.xodr'):
with open(map_path) as odr_file:
self.world = self.client.generate_opendrive_world(odr_file.read())
else:
raise RuntimeError(f'CARLA only supports OpenDrive maps')
self.timestep = timestep
self.tm = self.client.get_trafficmanager()
self.tm.set_synchronous_mode(True)
# Set to synchronous with fixed timestep
settings = self.world.get_settings()
settings.synchronous_mode = True
settings.fixed_delta_seconds = timestep # NOTE: Should not exceed 0.1
self.world.apply_settings(settings)
verbosePrint('Map loaded in simulator.')
self.render = render # visualization mode ON/OFF
self.record = record # whether to use the carla recorder
self.scenario_number = 0 # Number of the scenario executed
def compileStream(stream, namespace, filename='<stream>'):
"""Compile a stream of Scenic code and execute it in a namespace.
The compilation procedure consists of the following main steps:
1. Tokenize the input using the Python tokenizer.
2. Partition the tokens into blocks separated by import statements.
3. Translate Scenic constructions into valid Python syntax.
4. Parse the resulting Python code into an AST using the Python parser.
5. Modify the AST to achieve the desired semantics for Scenic.
6. Compile and execute the modified AST.
7. After executing all blocks, extract the global state (e.g. objects).
"""
if verbosity >= 2:
veneer.verbosePrint(f' Compiling Scenic module from {filename}...')
startTime = time.time()
# Tokenize input stream
try:
tokens = list(tokenize.tokenize(stream.readline))
except tokenize.TokenError as e:
line = e.args[1][0] if isinstance(e.args[1], tuple) else e.args[1]
raise TokenParseError(line, 'file ended during multiline string or expression')
# Partition into blocks with all imports at the end (since imports could
# pull in new constructor (Scenic class) definitions, which change the way
# subsequent tokens are transformed)
blocks = partitionByImports(tokens)
veneer.activate()
try:
# Execute preamble
exec(compile(preamble, '<veneer>', 'exec'), namespace)
# Execute each block
for blockNum, block in enumerate(blocks):
# Find all custom constructors defined so far
constructors = findConstructorsIn(namespace)
# Translate tokens to valid Python syntax
translator = TokenTranslator(constructors)
newSource, lineMap, allConstructors = translator.translate(block)
if dumpTranslatedPython:
print(f'### Begin translated Python from block {blockNum} of {filename}')
print(newSource)
print('### End translated Python')
# Parse the translated source
tree = parseTranslatedSource(newSource, lineMap, filename)
# Modify the parse tree to produce the correct semantics
newTree, requirements = translateParseTree(tree, lineMap, allConstructors)
if dumpFinalAST:
print(f'### Begin final AST from block {blockNum} of {filename}')
print(ast.dump(newTree, include_attributes=True))
print('### End final AST')
# Compile the modified tree
code = compileTranslatedTree(newTree, lineMap, filename)
# Execute it
executeCodeIn(code, namespace, lineMap, filename)
# Extract scenario state from veneer and store it
storeScenarioStateIn(namespace, requirements, lineMap, filename)
finally:
veneer.deactivate()
if verbosity >= 2:
totalTime = time.time() - startTime
veneer.verbosePrint(f' Compiled Scenic module in {totalTime:.4g} seconds.')
return namespace
def __init__(self,
carla_map,
address='127.0.0.1',
port=2000,
timeout=10,
render=True,
record=False,
timestep=0.1):
super().__init__()
verbosePrint('Connecting to CARLA...')
self.client = carla.Client(address, port)
self.client.set_timeout(
timeout) # limits networking operations (seconds)
self.world = self.client.load_world(carla_map)
self.map = carla_map
self.timestep = timestep
# Set to synchronous with fixed timestep
settings = self.world.get_settings()
settings.synchronous_mode = True
settings.fixed_delta_seconds = timestep # NOTE: Should not exceed 0.1
self.world.apply_settings(settings)
verbosePrint('Map loaded in simulator.')
self.render = render # visualization mode ON/OFF
self.record = record # whether to save images to disk
def polygonWithPoints(points):
polygon = shapely.geometry.Polygon(points)
if not polygon.is_valid: # TODO improve hack?
verbosePrint(f'WARNING: simplifying invalid polygon with points {points}')
polygon = polygon.simplify(0.5)
if not polygon.is_valid:
raise RuntimeError(f'unable to simplify polygon {polygon}')
return polygon
def coerceToAny(thing, types, error):
"""Coerce something into any of the given types, printing an error if impossible."""
for ty in types:
if canCoerce(thing, ty):
return coerce(thing, ty, error)
from scenic.syntax.veneer import verbosePrint
verbosePrint(f'Failed to coerce {thing} of type {underlyingType(thing)} to {types}',
file=sys.stderr)
raise RuntimeParseError(error)
def __init__(self, attrs):
super().__init__(attrs)
self.translation = attrs['translation']
points = list(p + self.translation for p in attrs['shape'])
if len(points) > 0:
self.points = [(x, z) for x, y, z in points]
self.region = PolygonalRegion(self.points)
else:
verbosePrint(
f'WARNING: Crossroad {self.osmID} has empty shape field!')
self.region = None
def __init__(self, attrs):
super().__init__(attrs)
self.translation = attrs['translation']
points = list(np.array(webotsToScenicPosition(p + self.translation))
for p in attrs['shape'])
if len(points) > 0:
self.points = points
self.region = PolygonalRegion(self.points)
else:
verbosePrint(f'WARNING: Crossroad {self.osmID} has empty shape field!')
self.region = None
def fromFile(path):
startTime = time.time()
with numpy.load(path, allow_pickle=True) as data:
Ax, Ay, Bx, By, sizeX, sizeY = data['misc']
m = Map(None, Ax, Ay, Bx, By)
m.sizeX, m.sizeY = sizeX, sizeY
m.displayImage = data['displayImage']
m.edgeData = { tuple(e): center_detection.EdgeData(*rest) for e, *rest in data['edges'] }
m.orderedCurbPoints = list(m.edgeData.keys())
m.edgeTree = scipy.spatial.cKDTree(m.orderedCurbPoints) # rebuild k-D tree
m.roadArray = data['roadArray']
totalTime = time.time() - startTime
verbosePrint(f'Loaded GTA map in {totalTime:.2f} seconds.')
return m
def fromPickle(cls, path, originalDigest=None):
startTime = time.time()
verbosePrint('Loading cached version of road network...')
with open(path, 'rb') as f:
versionField = f.read(4)
if len(versionField) != 4:
raise pickle.UnpicklingError(f'{cls.pickledExt} file is corrupted')
version = struct.unpack('<I', versionField)
if version[0] != cls._currentFormatVersion():
raise pickle.UnpicklingError(f'{cls.pickledExt} file is too old; '
'regenerate it from the original map')
digest = f.read(64)
if len(digest) != 64:
raise pickle.UnpicklingError(f'{cls.pickledExt} file is corrupted')
if originalDigest and originalDigest != digest:
raise cls.DigestMismatchError(
f'{cls.pickledExt} file does not correspond to the original map; '
' regenerate it'
)
with gzip.open(f) as gf:
try:
network = pickle.load(gf)
except pickle.UnpicklingError:
raise # propagate unpickling errors
except Exception as e:
# convert various other ways unpickling can fail into a more
# standard exception
raise pickle.UnpicklingError('unpickling failed') from e
# Reconnect links between network elements
def reconnect(thing):
state = thing.__dict__
for key, value in state.items():
if isinstance(value, _ElementPlaceholder):
state[key] = network.elements[value.uid]
proxy = weakref.proxy(network)
for elem in network.elements.values():
reconnect(elem)
elem.network = proxy
for elem in itertools.chain(network.lanes, network.intersections):
for maneuver in elem.maneuvers:
reconnect(maneuver)
totalTime = time.time() - startTime
verbosePrint(f'Loaded cached network in {totalTime:.2f} seconds.')
return network
def fromOpenDrive(cls, path, ref_points:int = 20, tolerance:float = 0.05,
fill_gaps:bool = True, fill_intersections:bool = True,
elide_short_roads:bool = False):
"""Create a `Network` from an OpenDRIVE file.
Args:
path: Path to the file, as in `Network.fromFile`.
ref_points: Number of points to discretize continuous reference lines
into.
tolerance: Tolerance for merging nearby geometries.
fill_gaps: Whether to attempt to fill gaps between adjacent lanes.
fill_intersections: Whether to attempt to fill gaps inside
intersections.
elide_short_roads: Whether to attempt to fix geometry artifacts by
eliding roads with length less than **tolerance**.
"""
import scenic.formats.opendrive.xodr_parser as xodr_parser
road_map = xodr_parser.RoadMap(tolerance=tolerance,
fill_intersections=fill_intersections,
elide_short_roads=elide_short_roads)
startTime = time.time()
verbosePrint('Parsing OpenDRIVE file...')
road_map.parse(path)
verbosePrint('Computing road geometry... (this may take a while)')
road_map.calculate_geometry(ref_points, calc_gap=fill_gaps, calc_intersect=True)
network = road_map.toScenicNetwork()
totalTime = time.time() - startTime
verbosePrint(f'Finished loading OpenDRIVE map in {totalTime:.2f} seconds.')
return network
def initApolloFor(self, obj, lgsvlObj):
"""Initialize Apollo for an ego vehicle.
Uses LG's interface which injects packets into Dreamview.
"""
if self.usingApollo:
raise RuntimeError('can only use one Apollo vehicle')
self.usingApollo = True
def on_collision(agent1, agent2, contact):
if agent1 is not None and agent1.name == lgsvlObj.name:
self.data[obj]['collision'] = True
if agent2 is not None and agent2.name == lgsvlObj.name:
self.data[obj]['collision'] = True
if self.data[obj]['collision']:
self.collisionOccurred = True
# Initialize Data
self.data[obj]['collision'] = False
lgsvlObj.on_collision(on_collision)
# connect bridge from LGSVL to Apollo
lgsvlObj.connect_bridge(obj.bridgeHost, obj.bridgePort)
# set up connection and map/vehicle configuration
import dreamview
dv = dreamview.Connection(self.client, lgsvlObj)
obj.dreamview = dv
waitToStabilize = False
hdMap = self.scene.params['apolloHDMap']
if dv.getCurrentMap() != hdMap:
dv.setHDMap(hdMap)
waitToStabilize = True
if dv.getCurrentVehicle() != obj.apolloVehicle:
dv.setVehicle(obj.apolloVehicle)
waitToStabilize = True
verbosePrint('Initializing Apollo...')
# stop the car to cancel buffered speed from previous simulations
cntrl = lgsvl.VehicleControl()
cntrl.throttle = 0.0
lgsvlObj.apply_control(cntrl, True)
# start modules
dv.disableModule('Control')
ready = dv.getModuleStatus()
for module in obj.apolloModules:
if not ready[module]:
dv.enableModule(module)
verbosePrint(f'Module {module} is not ready...')
waitToStabilize = True
while True:
ready = dv.getModuleStatus()
if all(ready[module] for module in obj.apolloModules):
break
# wait for Apollo to stabilize, if needed
if waitToStabilize:
verbosePrint('Waiting for Apollo to stabilize...')
self.client.run(25)
dv.enableModule('Control')
self.client.run(15)
verbosePrint('Initialized Apollo.')
def fromFile(cls, path, useCache:bool = True, writeCache:bool = True, **kwargs):
"""Create a `Network` from a map file.
This function calls an appropriate parsing routine based on the extension of the
given file. Supported map formats are:
* OpenDRIVE (``.xodr``): `Network.fromOpenDrive`
See the functions listed above for format-specific options to this function.
If no file extension is given in **path**, this function searches for any file
with the given name in one of the formats above (in order).
Args:
path: A string or other :term:`path-like object` giving a path to a file.
If no file extension is included, we search for any file type we know how
to parse.
useCache: Whether to use a cached version of the map, if one exists
and matches the given map file (default true; note that if the map file
changes, the cached version will still not be used).
writeCache: Whether to save a cached version of the processed map
after parsing has finished (default true).
kwargs: Additional keyword arguments specific to particular map formats.
Raises:
FileNotFoundError: no readable map was found at the given path.
ValueError: the given map is of an unknown format.
"""
path = pathlib.Path(path)
ext = path.suffix
handlers = { # in order of decreasing priority
'.xodr': cls.fromOpenDrive, # OpenDRIVE
# Pickled native representation; this is the lowest priority, since original
# maps should take precedence, but if the pickled version exists and matches
# the original, we'll use it.
cls.pickledExt: cls.fromPickle
}
if not ext: # no extension was given; search through possible formats
found = False
for ext in handlers:
newPath = path.with_suffix(ext)
if newPath.exists():
path = newPath
found = True
break
if not found:
raise FileNotFoundError(f'no readable maps found for path {path}')
elif ext not in handlers:
raise ValueError(f'unknown type of road network file {path}')
# If we don't have an underlying map file, return the pickled version directly
if ext == cls.pickledExt:
return cls.fromPickle(path)
# Otherwise, hash the underlying file to detect when the pickle is outdated
with open(path, 'rb') as f:
data = f.read()
digest = hashlib.blake2b(data).digest()
# By default, use the pickled version if it exists and is not outdated
pickledPath = path.with_suffix(cls.pickledExt)
if useCache and pickledPath.exists():
try:
return cls.fromPickle(pickledPath, originalDigest=digest)
except pickle.UnpicklingError:
verbosePrint('Unable to load cached network (old format or corrupted).')
except cls.DigestMismatchError:
verbosePrint('Cached network does not match original file; ignoring it.')
# Not using the pickled version; parse the original file based on its extension
network = handlers[ext](path, **kwargs)
if writeCache:
verbosePrint(f'Caching road network in {cls.pickledExt} file.')
network.dumpPickle(path.with_suffix(cls.pickledExt), digest)
return network
