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 test_overlap_area_no_overlap():
map = get_test_map()
l1 = map.laneletLayer.get(1)
l2 = map.laneletLayer.get(2)
l4 = map.laneletLayer.get(4)
overlap_area, overlap_centroid = LaneletHelpers.overlap_area(l1, l2)
assert overlap_area == 0
overlap_area, overlap_centroid = LaneletHelpers.overlap_area(l1, l4)
assert overlap_area == 0
def lanelet_oncoming_vehicles(self, frame, lanelet, lanelet_start_dist,
max_dist):
# get vehicles oncoming to a root lanelet
oncoming_vehicles = {}
for agent_id, agent in frame.agents.items():
if geometry.inside(lanelet, agent.point):
dist_along_lanelet = LaneletHelpers.dist_along(
lanelet, agent.point)
total_dist_along = lanelet_start_dist - dist_along_lanelet
if total_dist_along < max_dist:
oncoming_vehicles[agent_id] = (agent, total_dist_along)
if lanelet_start_dist < max_dist:
for prev_lanelet in self.routing_graph.previous(lanelet):
if self.traffic_rules.canPass(prev_lanelet):
prev_lanelet_start_dist = lanelet_start_dist + geometry.length2d(
prev_lanelet)
prev_oncoming_vehicles = self.lanelet_oncoming_vehicles(
frame, prev_lanelet, prev_lanelet_start_dist, max_dist)
for agent_id, (agent,
dist) in prev_oncoming_vehicles.items():
if agent_id not in oncoming_vehicles or dist < oncoming_vehicles[
agent_id][1]:
oncoming_vehicles[agent_id] = (agent, dist)
return oncoming_vehicles
def test_overlap_area_overlap():
map = get_test_map()
l4 = map.laneletLayer.get(4)
l5 = map.laneletLayer.get(5)
overlap_area, overlap_centroid = LaneletHelpers.overlap_area(l4, l5)
assert overlap_area == 0.5
assert overlap_centroid.x == 7 / 3
assert overlap_centroid.y == 5 / 3
def lanelet_crosses_lanelet(self, path_lanelet, lanelet):
# check if a lanelet crosses another lanelet, return overlap centroid
if path_lanelet != lanelet and self.traffic_rules.canPass(lanelet):
overlap_area, centroid = LaneletHelpers.overlap_area(
path_lanelet, lanelet)
split = (self.routing_graph.previous(path_lanelet) ==
self.routing_graph.previous(lanelet))
if overlap_area > 1 and not split:
return centroid
return None
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 angle_in_lane(state: AgentState, lanelet):
"""
Get the signed angle between the vehicle heading and the lane heading
Args:
state: current state of the vehicle
lanelet: : current lanelet of the vehicle
Returns: angle in radians
"""
lane_heading = LaneletHelpers.heading_at(lanelet, state.point)
angle_diff = np.diff(np.unwrap([lane_heading, state.heading]))[0]
return angle_diff
def oncoming_vehicles(self, route, frame, max_dist=30):
# get oncoming vehicles in lanes to cross
oncoming_vehicles = {}
lanelets_to_cross, crossing_points = self.lanelets_to_cross(route)
for lanelet, point in zip(lanelets_to_cross, crossing_points):
lanelet_start_dist = LaneletHelpers.dist_along(lanelet, point)
lanelet_oncoming_vehicles = self.lanelet_oncoming_vehicles(
frame, lanelet, lanelet_start_dist, max_dist)
for agent_id, (agent, dist) in lanelet_oncoming_vehicles.items():
if agent_id not in oncoming_vehicles or dist < oncoming_vehicles[
agent_id][1]:
oncoming_vehicles[agent_id] = (agent, dist)
return oncoming_vehicles
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 test_left_of_false():
left_lanelet, right_lanelet = get_adjacent_lanelets()
assert not LaneletHelpers.left_of(right_lanelet, left_lanelet)
def test_left_of_true():
left_lanelet, right_lanelet = get_adjacent_lanelets()
assert LaneletHelpers.left_of(left_lanelet, right_lanelet)
def test_following_lanelets_false():
lanelet_1, lanelet_2 = get_following_lanelets()
assert not LaneletHelpers.follows(lanelet_1, lanelet_2)
def test_following_lanelets_true():
lanelet_1, lanelet_2 = get_following_lanelets()
assert LaneletHelpers.follows(lanelet_2, lanelet_1)
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 can_pass(self, a, b):
# can we legally pass directly from lanelet a to b
return (a == b or LaneletHelpers.follows(b, a)
or self.traffic_rules.canChangeLane(a, b))
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 plot_test_map():
test_map = get_test_map()
for ll in test_map.laneletLayer:
LaneletHelpers.plot(ll)
plt.show()