def __init__(self, proj, data):
super().__init__(proj, data)
self.id = data['id']
self.direction = data['direction']
self.origin_lane = data['origin_lane']['lane_id']
medianPoints = localize(data['median'], proj)
self.median = PolylineRegion(medianPoints)
self.medianPoints = np.array(medianPoints)
self.region.orientation = VectorField(f'Road{self.id}Direction', self.directionAt)
def __attrs_post_init__(self):
proxy = weakref.proxy(self)
for uid, elem in self.elements.items():
assert elem.uid == uid
elem.network = proxy
self.allRoads = self.roads + self.connectingRoads
self.roadSections = tuple(sec for road in self.roads
for sec in road.sections)
self.laneSections = tuple(sec for lane in self.lanes
for sec in lane.sections)
if self.roadRegion is None:
self.roadRegion = PolygonalRegion.unionAll(self.roads)
if self.laneRegion is None:
self.laneRegion = PolygonalRegion.unionAll(self.lanes)
if self.intersectionRegion is None:
self.intersectionRegion = PolygonalRegion.unionAll(
self.intersections)
if self.crossingRegion is None:
self.crossingRegion = PolygonalRegion.unionAll(self.crossings)
if self.sidewalkRegion is None:
self.sidewalkRegion = PolygonalRegion.unionAll(self.sidewalks)
if self.shoulderRegion is None:
self.shoulderRegion = PolygonalRegion.unionAll(self.shoulders)
if self.drivableRegion is None:
self.drivableRegion = self.laneRegion.union(
self.intersectionRegion)
if self.walkableRegion is None:
self.walkableRegion = self.sidewalkRegion.union(
self.crossingRegion)
if self.curbRegion is None:
edges = []
for road in self.roads: # only include curbs of ordinary roads
if road.forwardLanes:
edges.append(road.forwardLanes.curb)
if road.backwardLanes:
edges.append(road.backwardLanes.curb)
self.curbRegion = PolylineRegion.unionAll(edges)
if self.roadDirection is None:
# TODO replace with a PolygonalVectorField for better pruning
self.roadDirection = VectorField('roadDirection',
self._defaultRoadDirection)
def computeGeometry(self, crossroads, snapTolerance=0.05):
## Approximate bounding polygon and sidewalks
# TODO improve this!!!
lefts, rights = [], []
leftSidewalk, rightSidewalk = [], []
headings = []
sidewalkWidths = itertools.chain(
self.sidewalkWidths, itertools.repeat(self.sidewalkWidths[-1]))
segments = zip(self.waypoints, sidewalkWidths)
for i, segment in enumerate(segments):
point, sidewalkWidth = segment
if i + 1 < len(self.waypoints):
nextPt = self.waypoints[i + 1]
dx, dy = nextPt - point
heading = normalizeAngle(math.atan2(dy, dx) - (math.pi / 2))
headings.append(heading)
perp = np.array([-dy, dx])
perp /= np.linalg.norm(perp)
else:
pass # use perp from last segment
toEdge = perp * (self.width / 2)
left = point + toEdge
right = point - toEdge
lefts.append(left)
rights.append(right)
toEdge = perp * sidewalkWidth
leftSidewalk.append(left + toEdge)
rightSidewalk.append(right - toEdge)
# Snap to adjacent crossroads if possible
if snapTolerance > 0:
sc = self.startCrossroad
if sc is not None:
if sc not in crossroads:
raise RuntimeError(
f'Road {self.osmID} begins at invalid crossroad {sc}')
crossroad = crossroads[sc]
if crossroad.region is not None:
pt = shapely.geometry.Point(lefts[0])
pt = shapely.ops.snap(pt, crossroad.region.polygons,
snapTolerance)
lefts[0] = np.array([pt.x, pt.y])
pt = shapely.geometry.Point(rights[0])
pt = shapely.ops.snap(pt, crossroad.region.polygons,
snapTolerance)
rights[0] = np.array([pt.x, pt.y])
perp = lefts[0] - rights[0]
toEdge = perp * (self.sidewalkWidths[0] /
np.linalg.norm(perp))
leftSidewalk[0] = lefts[0] + toEdge
rightSidewalk[0] = rights[0] - toEdge
ec = self.endCrossroad
if ec is not None:
if ec not in crossroads:
raise RuntimeError(
f'Road {self.osmID} ends at invalid crossroad {ec}')
crossroad = crossroads[ec]
if crossroad.region is not None:
pt = shapely.geometry.Point(lefts[-1])
pt = shapely.ops.snap(pt, crossroad.region.polygons,
snapTolerance)
lefts[-1] = np.array([pt.x, pt.y])
pt = shapely.geometry.Point(rights[-1])
pt = shapely.ops.snap(pt, crossroad.region.polygons,
snapTolerance)
rights[-1] = np.array([pt.x, pt.y])
perp = lefts[-1] - rights[-1]
toEdge = perp * (self.sidewalkWidths[-1] /
np.linalg.norm(perp))
leftSidewalk[-1] = lefts[-1] + toEdge
rightSidewalk[-1] = rights[-1] - toEdge
roadPoints = lefts + list(reversed(rights))
self.leftCurb = PolylineRegion(reversed(lefts))
self.rightCurb = PolylineRegion(rights)
self.leftSidewalk = self.rightSidewalk = None
if self.hasLeftSidewalk:
points = lefts + list(reversed(leftSidewalk))
polygon = polygonWithPoints(points)
assert polygon.is_valid, self.waypoints
self.leftSidewalk = PolygonalRegion(polygon=polygon)
if self.hasRightSidewalk:
points = rights + list(reversed(rightSidewalk))
polygon = polygonWithPoints(points)
assert polygon.is_valid, self.waypoints
self.rightSidewalk = PolygonalRegion(polygon=polygon)
## Compute lanes and traffic directions
cells = []
la, ra = lefts[0], rights[0]
gapA = (ra - la) / self.lanes
markerA = ra
laneMarkers = [[] for lane in range(self.lanes)]
for lb, rb, heading in zip(lefts[1:], rights[1:], headings):
# Compute lanes for this segment of road
gapB = (rb - lb) / self.lanes
markerB = rb
for lane, markers in enumerate(laneMarkers):
forward = lane < self.forwardLanes
if self.driveOnLeft:
forward = not forward
nextMarkerA = markerA - gapA
nextMarkerB = markerB - gapB
markers.append(nextMarkerA)
cell = shapely.geometry.Polygon(
(markerA, markerB, nextMarkerB, nextMarkerA))
heading = heading if forward else normalizeAngle(heading +
math.pi)
cells.append((cell, heading))
markerA = nextMarkerA
markerB = nextMarkerB
gapA = gapB
markerA = rb
self.lanes = []
markerB = rb
rightEdge = rights
for lane, markers in enumerate(laneMarkers):
markerB = markerB - gapB
markers.append(markerB)
self.lanes.append(
PolygonalRegion(rightEdge + list(reversed(markers))))
rightEdge = markers
self.laneMarkers = laneMarkers[:-1]
self.cells = cells
self.direction = PolygonalVectorField(f'Road{self.osmID}Direction',
cells)
roadPolygon = polygonWithPoints(roadPoints)
self.region = PolygonalRegion(polygon=roadPolygon,
orientation=self.direction)
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
self.curbsRegion = nowhere
else:
roadPoly = polygonUnion(road.region.polygons for road in self.roads)
self.roadsRegion = PolygonalRegion(polygon=roadPoly,
orientation=self.roadDirection)
drivableAreas.append(roadPoly)
curbs = [road.leftCurb.lineString for road in self.roads]
curbs.extend(road.rightCurb.lineString for road in self.roads)
curbLine = shapely.ops.unary_union(curbs)
self.curbsRegion = PolylineRegion(polyline=curbLine)
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)