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)