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))
