def get_current_lanelet(self, state, previous_lanelet=None):
point = state.point
radius = 3
bounding_box = BoundingBox2d(
BasicPoint2d(point.x - radius, point.y - radius),
BasicPoint2d(point.x + radius, point.y + radius))
nearby_lanelets = self.lanelet_map.laneletLayer.search(bounding_box)
best_lanelet = None
best_angle_diff = None
best_dist = None
best_can_pass = False
for lanelet in nearby_lanelets:
if self.traffic_rules.canPass(lanelet):
dist_from_point = geometry.distance(lanelet, point)
angle_diff = abs(self.angle_in_lane(state, lanelet))
can_pass = (False if previous_lanelet is None else
self.can_pass(previous_lanelet, lanelet))
if (angle_diff < np.pi / 2
and (best_lanelet is None or
(can_pass and not best_can_pass) or
((can_pass or not best_can_pass) and
(dist_from_point < best_dist or
(best_dist == dist_from_point
and angle_diff < best_angle_diff))))):
best_lanelet = lanelet
best_angle_diff = angle_diff
best_dist = dist_from_point
best_can_pass = can_pass
return best_lanelet
def part3lanelet_map():
# lanelets map work just as you would expect:
map = LaneletMap()
lanelet = get_a_lanelet()
map.add(lanelet)
assert lanelet in map.laneletLayer
assert map.pointLayer
assert not map.areaLayer
assert len(map.pointLayer.nearest(BasicPoint2d(0, 0), 1)) == 1
searchBox = BoundingBox2d(BasicPoint2d(0, 0), BasicPoint2d(2, 2))
assert len(map.pointLayer.search(searchBox)) > 1
# you can also create a map from a list of primitives (replace Lanelets by the other types)
mapBulk = lanelet2.core.createMapFromLanelets([get_a_lanelet()])
assert len(mapBulk.laneletLayer) == 1
def route_to_goal(self, start_lanelet, goal):
goal_point = BasicPoint2d(goal[0], goal[1])
end_lanelets = self.lanelets_at(goal_point)
best_route = None
for end_lanelet in end_lanelets:
route = self.routing_graph.getRoute(start_lanelet, end_lanelet)
if (route is not None and route.shortestPath()[-1] == end_lanelet
and (best_route is None
or route.length2d() < best_route.length2d())):
best_route = route
return best_route
def goal_type(self, state, goal, route, goal_types=None):
if goal_types is not None and len(goal_types) == 1:
return goal_types[0]
# get the goal type, based on the route
goal_point = BasicPoint2d(*goal)
path = route.shortestPath()
start_heading = state.heading
end_heading = LaneletHelpers.heading_at(path[-1], goal_point)
angle_to_goal = np.diff(np.unwrap([end_heading, start_heading]))[0]
if -np.pi / 8 < angle_to_goal < np.pi / 8:
return 'straight-on'
elif np.pi / 8 <= angle_to_goal < np.pi * 3 / 4:
return 'turn-right'
elif -np.pi / 8 >= angle_to_goal > np.pi * -3 / 4:
return 'turn-left'
else:
return 'u-turn'
def path_to_point_length(state, point, route):
path = route.shortestPath()
end_lanelet = path[-1]
end_lanelet_dist = LaneletHelpers.dist_along(end_lanelet, point)
start_point = BasicPoint2d(state.x, state.y)
start_lanelet = path[0]
start_lanelet_dist = LaneletHelpers.dist_along(start_lanelet,
start_point)
dist = end_lanelet_dist - start_lanelet_dist
if len(path) > 1:
prev_lanelet = start_lanelet
for idx in range(len(path) - 1):
lanelet = path[idx]
lane_change = (prev_lanelet.leftBound == lanelet.rightBound
or prev_lanelet.rightBound == lanelet.leftBound)
if not lane_change:
dist += geometry.length2d(lanelet)
return dist
def test_lanelet_map_search(self):
map = getLaneletMap()
nearest = map.laneletLayer.nearest(BasicPoint2d(0, 0), 1)
self.assertEqual(len(nearest), 1)
self.assertTrue(map.laneletLayer.exists(nearest[0].id))
def test_heading_at_center_curved():
lanelet = get_test_lanelet_curved()
point = BasicPoint2d(1.5, 0.75)
assert LaneletHelpers.heading_at(lanelet,
point) == pytest.approx(np.pi / 4)
def test_heading_at_center_end():
lanelet = get_test_lanelet_straight()
point = BasicPoint2d(1., 0.75)
assert LaneletHelpers.heading_at(lanelet, point) == pytest.approx(0)
def test_dist_along():
lanelet = get_test_lanelet_straight()
point = BasicPoint2d(0.5, 0.75)
assert LaneletHelpers.dist_along(lanelet, point) == pytest.approx(0.5)
def shapely_point_to_lanelet(p):
return BasicPoint2d(p.x, p.y)
def path_to_goal_length(cls, state, goal, route):
end_point = BasicPoint2d(*goal)
return cls.path_to_point_length(state, end_point, route)
def get_goal_routes(self, state: AgentState, goals):
"""
get most likely current lanelet and corresponding route for each goal
lanelet must be:
* close (radius 3m)
* direction within 90 degrees of car (reduce this?)
* can pass
* goal is reachable from
Rank lanelets based on:
* distance to current point
* angle diff
"""
point = state.point
radius = 3
bounding_box = BoundingBox2d(
BasicPoint2d(point.x - radius, point.y - radius),
BasicPoint2d(point.x + radius, point.y + radius))
nearby_lanelets = [
l for l in self.lanelet_map.laneletLayer.search(bounding_box)
if len(l.centerline) > 0
]
angle_diffs = [
abs(self.angle_in_lane(state, l)) for l in nearby_lanelets
]
dists_from_point = [
geometry.distance(l, point) for l in nearby_lanelets
]
# filter out invalid lanelets
possible_lanelets = []
for idx, lanelet in enumerate(nearby_lanelets):
if (angle_diffs[idx] < np.pi / 4 and dists_from_point[idx] < radius
and self.traffic_rules.canPass(lanelet)):
possible_lanelets.append(idx)
# find best lanelet for each goal
goal_lanelets = []
goal_routes = []
for goal in goals:
# find reachable lanelets for each goal
best_idx = None
best_route = None
for lanelet_idx in possible_lanelets:
if (best_idx is None or dists_from_point[lanelet_idx] <
dists_from_point[best_idx] or
(dists_from_point[lanelet_idx]
== dists_from_point[best_idx]
and angle_diffs[lanelet_idx] < angle_diffs[best_idx])):
lanelet = nearby_lanelets[lanelet_idx]
route = self.route_to_goal(lanelet, goal)
if route is not None:
best_idx = lanelet_idx
best_route = route
if best_idx is None:
goal_lanelet = None
else:
goal_lanelet = nearby_lanelets[best_idx]
goal_lanelets.append(goal_lanelet)
goal_routes.append(best_route)
return goal_routes
def point(self):
return BasicPoint2d(self.x, self.y)
def test_lanelets_at_none():
feature_extractor = get_feature_extractor()
lanelets = feature_extractor.lanelets_at(BasicPoint2d(1.0, 3.0))
assert len(lanelets) == 0
def test_lanelets_at_multiple():
feature_extractor = get_feature_extractor()
lanelets = feature_extractor.lanelets_at(BasicPoint2d(2.2, 1.5))
assert Counter([l.id for l in lanelets]) == Counter([4, 5])
def test_lanelets_at_single():
feature_extractor = get_feature_extractor()
lanelets = feature_extractor.lanelets_at(BasicPoint2d(1.0, 0.5))
assert [l.id for l in lanelets] == [1]