def RelativeHeading(X, Y=None): X = toHeading(X, '"relative heading of X from Y" with X not a heading') if Y is None: Y = ego().heading else: Y = toHeading(Y, '"relative heading of X from Y" with Y not a heading') return normalizeAngle(X - Y)
def relativeHeadingRange(baseHeading, offsetL, offsetR, targetHeading, tOffsetL, tOffsetR): if baseHeading is None or targetHeading is None: # heading may not be constant within cell return -math.pi, math.pi lower = normalizeAngle(baseHeading + offsetL) upper = normalizeAngle(baseHeading + offsetR) points = [lower, upper] if upper < lower: points.extend((math.pi, -math.pi)) tLower = normalizeAngle(targetHeading + tOffsetL) tUpper = normalizeAngle(targetHeading + tOffsetR) tPoints = [tLower, tUpper] if tUpper < tLower: tPoints.extend((math.pi, -math.pi)) rhs = [tp - p for tp in tPoints for p in points] # TODO improve return min(rhs), max(rhs)
def angleWith(self, other) -> float: """Compute the signed angle between self and other. The angle is positive if other is counterclockwise of self (considering the smallest possible rotation to align them). """ x, y = self.x, self.y ox, oy = other.x, other.y return normalizeAngle(math.atan2(oy, ox) - math.atan2(y, x))
def RelativeHeading(X, Y=None): """The 'relative heading of <heading> [from <heading>]' operator. If the 'from <heading>' is omitted, the heading of ego is used. """ X = toHeading(X, '"relative heading of X from Y" with X not a heading') if Y is None: Y = ego().heading else: Y = toHeading(Y, '"relative heading of X from Y" with Y not a heading') return normalizeAngle(X - Y)
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 angleTo(self, other): dx, dy = other.toVector() - self return normalizeAngle(math.atan2(dy, dx) - (math.pi / 2))
def webotsToScenicRotation(rot, tolerance2D=0): axis = np.array(rot[:3]) angle = rot[3] if np.linalg.norm(axis - (0, 1, 0)) > tolerance2D: return None return normalizeAngle(angle + math.pi)
def carlaToScenicHeading(rot): return normalizeAngle(-math.radians(rot.yaw + 90))
def lgsvlToScenicRotation(rot): """Convert LGSVL rotations to Scenic headings. Drops all but the Y component. """ return normalizeAngle(-math.radians(rot.y))