def get_distance_closest_node_route(self, pos, route):
distance = []
for node_iter in route:
if node_iter in self._map.get_intersection_nodes():
distance.append(sldist(node_iter, pos))
if not distance:
return sldist(route[-1], pos)
return sorted(distance)[0]
def get_distance_closest_node_route(self, pos, route):
distance = []
for node_iter in route:
if node_iter in self._map.get_intersection_nodes():
distance.append(sldist(node_iter, pos))
if not distance:
return sldist(route[-1], pos)
return sorted(distance)[0]
def _closest_intersection_position(self, current_node):
distance_vector = []
for node_iterator in self._map.get_intersection_nodes():
distance_vector.append(sldist(node_iterator, current_node))
return sorted(distance_vector)[0]
def get_distance_closest_node(self, pos):
distance = []
for node_iter in self._graph.intersection_nodes():
distance.append(sldist(node_iter, pos))
return sorted(distance)[0]
def get_distance_closest_node(self, pos):
distance = []
for node_iter in self._graph.intersection_nodes():
distance.append(sldist(node_iter, pos))
return sorted(distance)[0]
def _closest_intersection_position(self, current_node):
distance_vector = []
for node_iterator in self._map.get_intersection_nodes():
distance_vector.append(sldist(node_iterator, current_node))
return sorted(distance_vector)[0]
def _closest_intersection_route_position(self, current_node, route):
#print("current node is", current_node, "route is", route)
distance_vector = []
for _ in route:
for node_iterator in self._map.get_intersection_nodes():
#print("a intersection node is", node_iterator)
distance_vector.append(sldist(node_iterator, current_node))
return sorted(distance_vector)[0]
def get_upcoming_traffic_light(self,
measurement,
sensor_data,
thresh_meters=15.):
"""
Algorithm:
1) snap car location to closest road node
2) snap car orientation to closest NORTH/SOUTH/EAST/WEST discretization
3) snap all traffic lights to the closest intersection-road nodes
4) look ahead from car's node position until it intersects with a snapped-traffic light. There are three traffic lights there, so use the car's + traffic lights orientation to determine which traffic light (of 3) is the relevent one at that intersection
5) if look ahead has not traffic lights ahead (e.g. corners of map), then return `None`
6) if in the middle of intersection (determined by pixel value of `CARLA_0.8.4/PythonClient/carla/planner/TownXXLanes.png`) then return `None` traffic light data (but with a string that specifics because it's `"INTERSECTION"`.
:param measurement: what the carla client reads from client.read_data()
:param sensor_data: IGNORED
:param thresh_meters:
:return: traffic_light_state (str): either "GREEN", "YELLOW", "RED", "INTERSECTION", or "NONE".
traffic_light_data: carla traffic_light object (None if no traffic light ahead); and
.id: (int)
.traffic_light
.transform
.location
.x: (float)
.y: (float)
.z: (float)
.orientation
.x: (float) in [-1,1]
.y: (float) in [-1,1]
.rotation
.yaw: (float) in degrees
state: (int) 0 mean GREEN, 1 means YELLOW, 2 means RED
"""
player_world = self._get_world(measurement.player_measurements)
in_intersection = self.in_intersetion(player_world)
if in_intersection:
if self.last_traffic_light_state == 'RED':
# If the traffic light was RED before, and we entered the intersection, then we entered during a red light!
self.red_light_violations += 1
if self.last_traffic_light_state != 'INTERSECTION':
# We switched from non-intersection to INTERSECTION. Every time this happens, count it as a new intersection.
self.intersection_count += 1
self.last_traffic_light_state = "INTERSECTION"
return self.last_traffic_light_state, None
round_ori = lambda ori: tuple(map(
round, ori)) # because we'll be doing equality comparisons
rotate180 = lambda ori: (-ori[0], -ori[1])
node_on_map = lambda node: 0 <= node[0] < self._graph._resolution[
0] and 0 <= node[1] < self._graph._resolution[1]
next_node = lambda node, ori: (node[0] + ori[0], node[1] + ori[1])
# where are the traffic lights (to the closest intersection node)
traffic_lights = [
_ for _ in measurement.non_player_agents
if _.HasField('traffic_light')
]
if self.traffic_light_inodes_and_oris is None: # then compute it (we only need to compute it once)
tl_nodes = [
self.convert(self._get_world(tl.traffic_light), 'node')
for tl in traffic_lights
]
correct_traffic_light_ori = lambda x: (-x[1], x[
0]) # to point in direction the light faces
tl_oris_round = [
correct_traffic_light_ori(
round_ori(self._get_ori(tl.traffic_light)))
for tl in traffic_lights
]
# not all traffic lights are at roads or intersection nodes, let's snap to closest intersection node
intersection_nodes = self.get_intersection_nodes(
) # "inode" for short
closest_intersection_node = lambda node: min(
intersection_nodes, key=lambda inode: sldist(inode, node))
tl_inodes = list(map(closest_intersection_node, tl_nodes))
self.traffic_light_inodes_and_oris = list(
map(lambda x, y: x + y, tl_inodes, tl_oris_round)) # 36, 4
try:
player_road_node = self.get_closest_road_node(player_world)
except IndexError:
self.last_traffic_light_state = 'NONE'
return self.last_traffic_light_state, None
player_ori_round = round_ori(
self._get_ori(measurement.player_measurements))
target_traffic_light_ori_round = rotate180(player_ori_round)
vector3_to_np = lambda v: np.array([v.x, v.y, v.z])
# now look for closest traffic light in this direction
node_ahead = player_road_node
# tform_now = transform.Transform(measurement.player_measurements.transform).inverse()
while node_on_map(node_ahead):
key = node_ahead + target_traffic_light_ori_round # concat for tuple of 4 elements
if key in self.traffic_light_inodes_and_oris:
# found this traffic light ahead!
i = self.traffic_light_inodes_and_oris.index(key)
traffic_light_data = traffic_lights[i]
data = vector3_to_np(
traffic_light_data.traffic_light.transform.location)
# tform_now.transform_points(data)
distance_to_player = np.linalg.norm(player_world - data)
log.debug("Distance of light to player: {:.1f}".format(
distance_to_player))
if distance_to_player > thresh_meters:
# Light is too far away!
self.last_traffic_light_state = "NONE"
log.debug("Light is too far, does not apply")
return self.last_traffic_light_state, None
else:
traffic_light_state_int = traffic_light_data.traffic_light.state # (int) either: 0, 1, 2
traffic_light_state_str = ["GREEN", "YELLOW",
"RED"][traffic_light_state_int]
self.last_traffic_light_state = traffic_light_state_str
log.debug(
"Light is close enough, applies. State={}. Str='{}'".
format(traffic_light_data.traffic_light.state,
self.last_traffic_light_state))
return self.last_traffic_light_state, traffic_light_data
node_ahead = next_node(node_ahead, player_ori_round)
else:
self.last_traffic_light_state = "NONE"
return self.last_traffic_light_state, None
