def __init__(self, intersection):
self.intersection = intersection
drivablePoly = polygonUnion(gw.region.polygons
for gw in intersection.vehicleGuideways)
walkablePoly = polygonUnion(cw.region.polygons
for cw in intersection.crosswalks)
workspacePoly = polygonUnion((drivablePoly, walkablePoly))
self.roadDirection = VectorField('RoadDirection', intersection.directionAt)
self.drivableRegion = PolygonalRegion(polygon=drivablePoly, orientation=self.roadDirection)
self.workspaceRegion = PolygonalRegion(polygon=workspacePoly)
super().__init__(self.workspaceRegion)
def union(self, other):
poly = toPolygon(other)
if not poly:
raise RuntimeError(
f'cannot take union of PolygonalRegion with {other}')
union = polygonUnion((self.polygons, poly))
return PolygonalRegion(polygon=union)
def union(self, other, triedReversed=False, buf=0):
poly = toPolygon(other)
if not poly:
return super().union(other, triedReversed)
union = polygonUnion((self.polygons, poly), buf=buf)
orientation = VectorField.forUnionOf((self, other))
return PolygonalRegion(polygon=union, orientation=orientation)
def unionAll(regions, buf=0):
regs, polys = [], []
for reg in regions:
if reg != nowhere:
regs.append(reg)
polys.append(toPolygon(reg))
if not polys:
return nowhere
if any(not poly for poly in polys):
raise RuntimeError(f'cannot take union of regions {regions}')
union = polygonUnion(polys, buf=buf)
orientation = VectorField.forUnionOf(regs)
return PolygonalRegion(polygon=union, orientation=orientation)
def feasibleRHPolygon(field, offsetL, offsetR,
tField, tOffsetL, tOffsetR,
lowerBound, upperBound, maxDist):
"""Find where objects aligned to the given fields can satisfy the given RH bounds."""
if (offsetR - offsetL >= math.tau
or tOffsetR - tOffsetL >= math.tau
or upperBound - lowerBound >= math.tau):
return None
polygons = []
expanded = [(poly.buffer(maxDist), heading) for poly, heading in tField.cells]
for baseCell, baseHeading in field.cells: # TODO skip cells not contained in base region?
for expandedTargetCell, targetHeading in expanded:
lower, upper = relativeHeadingRange(baseHeading, offsetL, offsetR,
targetHeading, tOffsetL, tOffsetR)
if (upper >= lowerBound and lower <= upperBound): # RH intervals overlap
intersection = baseCell & expandedTargetCell
if not intersection.is_empty:
assert isinstance(intersection, shapely.geometry.Polygon), intersection
polygons.append(intersection)
return polygonUnion(polygons)
def __init__(self, world):
# Find roads, crossroads, and pedestrian crossings
nodeClasses = {
'Road': Road,
'Crossroad': Crossroad,
'PedestrianCrossing': PedestrianCrossing
}
self.roads, self.crossroads, self.crossings = world_parser.findNodeTypesIn(
('Road', 'Crossroad', 'PedestrianCrossing'), world, nodeClasses)
# Compute road geometry
crossroadsByID = {
crossroad.osmID: crossroad
for crossroad in self.crossroads
}
for road in self.roads:
road.computeGeometry(crossroadsByID)
# Construct regions
allCells = []
drivableAreas = []
for road in self.roads:
assert road.region.polygons.is_valid, (road.waypoints,
road.region.points)
allCells.extend(road.cells)
for crossroad in self.crossroads:
if crossroad.region is not None:
for poly in crossroad.region.polygons:
allCells.append((poly, None))
if not allCells:
raise RuntimeError('Webots world has no drivable geometry!')
self.roadDirection = PolygonalVectorField(
'roadDirection',
allCells,
headingFunction=lambda pos: 0,
defaultHeading=0 # TODO fix
)
if not self.roads:
roadPoly = None
self.roadsRegion = nowhere
else:
roadPoly = polygonUnion(road.region.polygons
for road in self.roads)
self.roadsRegion = PolygonalRegion(polygon=roadPoly,
orientation=self.roadDirection)
drivableAreas.append(roadPoly)
if not self.crossroads:
crossroadPoly = None
self.crossroadsRegion = nowhere
else:
crossroadPoly = polygonUnion(cr.region.polygons
for cr in self.crossroads
if cr.region is not None)
self.crossroadsRegion = PolygonalRegion(
polygon=crossroadPoly, orientation=self.roadDirection)
drivableAreas.append(crossroadPoly)
sidewalks = []
walkableAreas = []
for road in self.roads:
if road.hasLeftSidewalk:
sidewalks.append(road.leftSidewalk.polygons)
if road.hasRightSidewalk:
sidewalks.append(road.rightSidewalk.polygons)
if not sidewalks:
sidewalksPoly = None
self.sidewalksRegion = nowhere
else:
sidewalksPoly = polygonUnion(sidewalks)
self.sidewalksRegion = regionWithPolygons(sidewalksPoly)
walkableAreas.append(sidewalksPoly)
if not self.crossings:
crossingsPoly = None
self.crossingsRegion = nowhere
else:
crossingsPoly = polygonUnion(crossing.region.polygons
for crossing in self.crossings)
self.crossingsRegion = regionWithPolygons(crossingsPoly)
walkableAreas.append(crossingsPoly)
if not walkableAreas:
self.walkableRegion = nowhere
else:
walkablePoly = polygonUnion(walkableAreas)
self.walkableRegion = regionWithPolygons(walkablePoly)
drivablePoly = polygonUnion(drivableAreas)
self.drivableRegion = PolygonalRegion(polygon=drivablePoly,
orientation=self.roadDirection)
workspacePoly = polygonUnion(drivableAreas + walkableAreas)
self.workspaceRegion = PolygonalRegion(polygon=workspacePoly)
# Identify various roads and lanes of interest
slowCurbs = []
for road in self.roads:
if road.forwardLanes > 1:
slowCurbs.append(road.rightCurb)
if road.backwardLanes > 1:
slowCurbs.append(road.leftCurb)
self.slowCurbs = slowCurbs
super().__init__(self.workspaceRegion)
