def lanelet_crosses_line(self, path_lanelet):
# check if the midline of a lanelet crosses a road marking
for lanelet in self.lanelet_map.laneletLayer:
if path_lanelet != lanelet and self.traffic_rules.canPass(lanelet):
left_virtual = ('type' in lanelet.leftBound.attributes
and lanelet.leftBound.attributes['type']
== 'virtual')
right_virtual = ('type' in lanelet.rightBound.attributes
and lanelet.rightBound.attributes['type']
== 'virtual')
path_centerline = geometry.to2D(path_lanelet.centerline)
right_bound = geometry.to2D(lanelet.rightBound)
left_bound = geometry.to2D(lanelet.leftBound)
left_intersects = (not left_virtual and geometry.intersects2d(
path_centerline, left_bound))
right_intersects = (not right_virtual
and geometry.intersects2d(
path_centerline, right_bound))
if path_lanelet != lanelet:
if left_intersects:
intersection_point = LaneletHelpers.intersection_point(
path_centerline, left_bound)
return lanelet, intersection_point
elif right_intersects:
intersection_point = LaneletHelpers.intersection_point(
path_centerline, right_bound)
return lanelet, intersection_point
else:
return None, None
def part1primitives():
# Primitives work very similar to c++, except that the data can be accessed as properties instead of functions
p = Point3d(getId(), 0, 0, 0)
assert p.x == 0
p.id = getId()
p.attributes["key"] = "value"
assert "key" in p.attributes
assert p.attributes["key"] == "v"
# the 2d/3d mechanics work too
p2d = geometry.to2D(p)
# all (common) geometry calculations are available as well:
p2 = Point3d(getId(), 1, 0, 0)
assert lanelet2.geometry.distance(p, p2) == 1
assert lanelet2.geometry.distance(p2d, Point2d(getId(), 1, 0, 1)) == 1
# linestrings work conceptually similar to a list (but they only accept points, of course)
ls = LineString3d(getId(), [p, p2])
assert ls[0] == p
assert ls[-1] == p2
assert p in ls
for pt in ls:
assert pt.y == 0
ls_inv = ls.invert()
assert ls_inv[0] == p2
ls.append(Point3d(getId(), 2, 0, 0))
del ls[2]
def heading_at(lanelet, point):
centerline = geometry.to2D(lanelet.centerline)
proj_point = geometry.project(centerline, point)
dist_along = LaneletHelpers.dist_along(lanelet, proj_point)
epsilon = 1e-4
if geometry.length2d(lanelet) - dist_along > epsilon:
# get a point after current point
next_point = geometry.interpolatedPointAtDistance(
centerline, dist_along + epsilon)
x1 = next_point.y - proj_point.y
x2 = next_point.x - proj_point.x
lane_heading = np.arctan2(x1, x2)
else:
# get a point before current point
prev_point = geometry.interpolatedPointAtDistance(
centerline, dist_along - epsilon)
x1 = proj_point.y - prev_point.y
x2 = proj_point.x - prev_point.x
lane_heading = np.arctan2(x1, x2)
return lane_heading
def test_intersects_l2l(self):
self.assertTrue(
intersects2d(to2D(getLineString()), to2D(getLineString())))
def test_intersection_l2l(self):
point_list = intersection(to2D(getLineString()), to2D(getLineString()))
self.assertEqual(point_list[0].x, 0.0)
self.assertEqual(point_list[0].y, 0.0)
def test_bounding_box_line(self):
bbox = boundingBox2d(to2D(getLineString()))
self.assertEqual(bbox.min.x, 0)
def dist_from_center(point, lanelet):
return geometry.toArcCoordinates(geometry.to2D(lanelet.centerline),
point).distance
def dist_along(lanelet, point):
centerline = geometry.to2D(lanelet.centerline)
return geometry.toArcCoordinates(centerline, point).length
def vehicle_point_in_lanelet(self):
point = project(to2D(self.lanelet.centerline), self.p2d)
return BasicPoint3d(point.x, point.y)
def point_in_lanelet(self, lanelet, p2d):
point = project(to2D(lanelet.centerline), p2d)
return BasicPoint3d(point.x, point.y)
