class BirdsEyeObservation(FluidsObs):
"""
Bird's-eye 2D top-down image centered on the vehicle, similar to what is visualized.
Minor difference is that drivable regions are colorless to differentiate from illegal drivable regions.
Array representation is (obs_dim, obs_dim, 3).
"""
def __init__(self, car, obs_dim=500):
from fluids.assets import Car, Lane, Sidewalk, Terrain, TrafficLight, Waypoint, PedCrossing, Pedestrian
state = car.state
self.car = car
self.grid_dim = obs_dim
self.grid_square = Shape(x=car.x+obs_dim/3*np.cos(car.angle),
y=car.y-obs_dim/3*np.sin(car.angle),
xdim=obs_dim, ydim=obs_dim, angle=car.angle,
color=None)
self.all_collideables = []
collideable_map = {Waypoint:[]}
for k, obj in iteritems(state.objects):
if (car.can_collide(obj) or type(obj) in {TrafficLight}) and self.grid_square.intersects(obj):
typ = type(obj)
if typ not in collideable_map:
collideable_map[typ] = []
collideable_map[typ].append(obj)
self.all_collideables.append(obj)
for waypoint in car.waypoints:
collideable_map[Waypoint].append(waypoint)
self.all_collideables.append(waypoint)
debug_window = pygame.Surface((self.grid_dim, self.grid_dim))
gd = self.grid_dim
a0 = self.car.angle + np.pi / 2
a1 = self.car.angle
for typ in [Terrain, Sidewalk, Lane, Car, TrafficLight, Waypoint, PedCrossing, Pedestrian]:
if typ in collideable_map:
for obj in collideable_map[typ]:
rel_obj = obj.get_relative((self.car.x+gd/2*np.cos(a0)-gd/6*np.cos(a1),
self.car.y-gd/2*np.sin(a0)+gd/6*np.sin(a1),
self.car.angle))
rel_obj.render(debug_window, border=None)
self.pygame_rep = pygame.transform.rotate(debug_window, 90)
def render(self, surface):
self.grid_square.render(surface)
if self.car.vis_level > 3:
if self.car.vis_level > 4:
for obj in self.all_collideables:
obj.render_debug(surface)
surface.blit(self.pygame_rep, (surface.get_size()[0] - self.grid_dim, 0))
pygame.draw.rect(surface, (0, 0, 0),
pygame.Rect((surface.get_size()[0] - self.grid_dim-5, 0-5),
(self.grid_dim+10, self.grid_dim+10)), 10)
def get_array(self):
arr = pygame.surfarray.array3d(self.pygame_rep)
return arr
class GridObservation(FluidsObs):
"""
Grid observation type.
Observation is an occupancy grid over the detection region.
Observation has 6 dimensions: terrain, drivable regions, illegal drivable regions, cars, pedestrians, and traffic lights.
Array representation is (grid_size, grid_size, 6)
"""
def __init__(self, car, obs_dim=500):
from fluids.assets import ALL_OBJS, TrafficLight, Lane, Terrain, Sidewalk, \
PedCrossing, Street, Car, Waypoint, Pedestrian
state = car.state
self.car = car
self.grid_dim = obs_dim
self.grid_square = Shape(x=car.x + obs_dim / 3 * np.cos(car.angle),
y=car.y - obs_dim / 3 * np.sin(car.angle),
xdim=obs_dim,
ydim=obs_dim,
angle=car.angle,
color=None)
self.all_collideables = []
collideable_map = {typ: [] for typ in ALL_OBJS}
for k, obj in iteritems(state.objects):
if (car.can_collide(obj)
or type(obj) in {TrafficLight, Lane, Street
}) and self.grid_square.intersects(obj):
typ = type(obj)
if typ not in collideable_map:
collideable_map[typ] = []
collideable_map[typ].append(obj)
self.all_collideables.append(obj)
for waypoint in car.waypoints:
collideable_map[Waypoint].append(waypoint)
self.all_collideables.append(waypoint)
terrain_window = pygame.Surface((self.grid_dim, self.grid_dim))
drivable_window = pygame.Surface((self.grid_dim, self.grid_dim))
undrivable_window = pygame.Surface((self.grid_dim, self.grid_dim))
car_window = pygame.Surface((self.grid_dim, self.grid_dim))
ped_window = pygame.Surface((self.grid_dim, self.grid_dim))
light_window = pygame.Surface((self.grid_dim, self.grid_dim))
direction_window = pygame.Surface((self.grid_dim, self.grid_dim))
gd = self.grid_dim
a0 = self.car.angle + np.pi / 2
a1 = self.car.angle
rel = (self.car.x + gd / 2 * np.cos(a0) - gd / 6 * np.cos(a1),
self.car.y - gd / 2 * np.sin(a0) + gd / 6 * np.sin(a1),
self.car.angle)
for typ in [Terrain, Sidewalk, PedCrossing]:
for obj in collideable_map[typ]:
rel_obj = obj.get_relative(rel)
rel_obj.render(terrain_window, border=None)
for obj in collideable_map[Lane]:
rel_obj = obj.get_relative(rel)
if not car.can_collide(obj):
rel_obj.render(drivable_window, border=None)
else:
rel_obj.render(undrivable_window, border=None)
for obj in collideable_map[Street]:
rel_obj = obj.get_relative(rel)
rel_obj.render(drivable_window, border=None)
for obj in collideable_map[Car]:
rel_obj = obj.get_relative(rel)
rel_obj.render(car_window, border=None)
for obj in collideable_map[Pedestrian]:
rel_obj = obj.get_relative(rel)
rel_obj.render(ped_window, border=None)
for obj in collideable_map[TrafficLight]:
rel_obj = obj.get_relative(rel)
rel_obj.render(light_window, border=None)
point = (int(gd / 6), int(gd / 2))
for p in self.car.waypoints:
relp = p.get_relative(rel)
new_point = int(relp.x), int(relp.y)
pygame.draw.line(direction_window, (255, 255, 255), point,
new_point, 10)
point = new_point
self.pygame_rep = [
pygame.transform.rotate(window, 90) for window in [
terrain_window, drivable_window, undrivable_window, car_window,
ped_window, light_window, direction_window
]
]
def render(self, surface):
self.grid_square.render(surface)
if self.car.vis_level > 3:
if self.car.vis_level > 4:
for obj in self.all_collideables:
obj.render_debug(surface)
for y in range(4):
for x in range(2):
i = y + x * 4
if i < len(self.pygame_rep):
surface.blit(self.pygame_rep[i],
(surface.get_size()[0] - self.grid_dim *
(x + 1), self.grid_dim * y))
pygame.draw.rect(
surface, (0, 100, 0),
pygame.Rect(
(surface.get_size()[0] - self.grid_dim *
(x + 1) - 5, 0 - 5 + self.grid_dim * y),
(self.grid_dim + 10, self.grid_dim + 10)), 10)
def get_array(self):
arr = np.zeros((self.grid_dim, self.grid_dim, len(self.pygame_rep)))
for i in range(len(self.pygame_rep)):
arr[:, :, i] = pygame.surfarray.array2d(self.pygame_rep[0]) > 0
return arr
class VecObservation(FluidsObs):
"""
Grid observation type.
Observation is an occupancy grid over the detection region.
Observation has 11 dimensions: terrain, drivable regions, illegal drivable
regions, cars, pedestrians, traffic lights x 3, way points, point trajectory and edge trajectory.
Array representation is (grid_size, grid_size, 11)
"""
def __init__(self, car, obs_dim=500, shape=(500, 500)):
from fluids.assets import ALL_OBJS, TrafficLight, Lane, Terrain, Sidewalk, \
PedCrossing, Street, Car, Waypoint, Pedestrian
state = car.state
self.car = car
self.shape = shape
self.grid_dim = obs_dim
self.downsample = self.shape != (obs_dim, obs_dim)
self.grid_square = Shape(x=car.x + obs_dim / 3 * np.cos(car.angle),
y=car.y - obs_dim / 3 * np.sin(car.angle),
xdim=obs_dim, ydim=obs_dim, angle=car.angle,
color=None, border_color=(200, 0, 0))
gd = self.grid_dim
a0 = self.car.angle + np.pi / 2
a1 = self.car.angle
# rel = (self.car.x + gd / 2 * np.cos(a0) - gd / 6 * np.cos(a1),
# self.car.y - gd / 2 * np.sin(a0) + gd / 6 * np.sin(a1),
# self.car.angle)
rel = (self.car.x,
self.car.y,
self.car.angle)
scale_factor = 500.0
# Gather the lights and cars objects for later information gathering
traffic_lights = []
other_cars = []
for k, obj in iteritems(state.objects):
if (car.can_collide(obj) or type(obj) in {TrafficLight, Lane, Street}) and self.grid_square.intersects(obj):
typ = type(obj)
if typ == TrafficLight:
traffic_lights.append(obj)
elif typ == Car:
other_cars.append(obj)
# Information about the stop light
light_x, light_y = 0.0, 0.0
color_to_state = {RED:-1.0, YELLOW: 0.0, GREEN: 1.0}
light_state = 1.0 # Default state to green so that if it doesn't exist the default behaviour is to go.
# 0 if no light 1 if light exists
light_exists = 0.0
closest_dist = 99999999
for light in traffic_lights:
rel_obj = light.get_relative(rel)
x, y = rel_obj.x, rel_obj.y
dist_sq = x**2 + y**2
if dist_sq < closest_dist:
closest_dist = dist_sq
# TODO: Scale so the values aren't as large
light_x, light_y = rel_obj.x, rel_obj.y
light_exists = 1.0
light_state = color_to_state[light.color]
light_x /= scale_factor
light_y /= scale_factor
# Information about the closest car
other_car_x, other_car_y = 0.0, 0.0
other_car_vx, other_car_vy = 0.0, 0.0
other_car_exists = 0.0
closest_dist = 99999999
for other_car in other_cars:
rel_obj = other_car.get_relative(rel)
x, y = rel_obj.x, rel_obj.y
dist_sq = x**2 + y**2
if dist_sq < closest_dist:
closest_dist = dist_sq
# TODO: Scale so the values aren't as large
other_car_x, other_car_y = rel_obj.x, rel_obj.y
other_car_exists = 1.0
relative_theta = other_car.angle - car.angle
other_car_vx = other_car.vel * np.cos(relative_theta)
other_car_vy = other_car.vel * -np.sin(relative_theta)
other_car_exists
other_car_x /= scale_factor
other_car_y /= scale_factor
# Information about the next waypoint
# TODO: Scale so the values aren't as large
if car.waypoints:
rel_waypoint = car.waypoints[0].get_relative(rel)
waypoint_x = rel_waypoint.x
waypoint_y = rel_waypoint.y
else:
waypoint_x, waypoint_y = 50.0, 0.0
waypoint_x /= scale_factor
waypoint_y /= scale_factor
self.pygame_rep = [light_x, light_y, light_state, light_exists, other_car_x, other_car_y, other_car_vx, other_car_vy, other_car_exists, waypoint_x, waypoint_y, car.vel]
# self.pygame_rep = [pygame.transform.rotate(window, 90) for window in [terrain_window,
# drivable_window,
# undrivable_window,
# car_window,
# ped_window,
# light_window_red,
# light_window_green,
# light_window_yellow,
# direction_window,
# direction_pixel_window,
# direction_edge_window
# ]]
def render(self, surface):
self.grid_square.render(surface, border=10)
if self.car.vis_level > 3:
if self.car.vis_level > 4:
for obj in self.all_collideables:
obj.render_debug(surface)
for y in range(4):
for x in range(2):
i = y + x * 4
if i < len(self.pygame_rep):
surface.blit(self.pygame_rep[i],
(surface.get_size()[0] - self.grid_dim * (x + 1), self.grid_dim * y))
pygame.draw.rect(surface, (200, 0, 0),
pygame.Rect((surface.get_size()[0] - self.grid_dim * (x + 1) - 5,
0 - 5 + self.grid_dim * y),
(self.grid_dim + 10, self.grid_dim + 10)), 10)
def get_array(self):
return self.pygame_rep
def sp_imresize(self, arr, shape):
return np.array([imresize(arr[:, :, i], shape) for i in range(arr.shape[2])]).T
class GridObservation(FluidsObs):
"""
Grid observation type.
Observation is an occupancy grid over the detection region.
Observation has 11 dimensions: terrain, drivable regions, illegal drivable
regions, cars, pedestrians, traffic lights x 3, way points, point trajectory and edge trajectory.
Array representation is (grid_size, grid_size, 11)
"""
def __init__(self, car, obs_dim=500, shape=(500, 500)):
from fluids.assets import ALL_OBJS, TrafficLight, Lane, Terrain, Sidewalk, \
PedCrossing, Street, Car, Waypoint, Pedestrian
state = car.state
self.car = car
self.shape = shape
self.grid_dim = obs_dim
self.downsample = self.shape != (obs_dim, obs_dim)
self.grid_square = Shape(x=car.x + obs_dim / 3 * np.cos(car.angle),
y=car.y - obs_dim / 3 * np.sin(car.angle),
xdim=obs_dim,
ydim=obs_dim,
angle=car.angle,
color=None,
border_color=(200, 0, 0))
self.all_collideables = []
collideable_map = {typ: [] for typ in ALL_OBJS}
for k, obj in iteritems(state.objects):
if (car.can_collide(obj)
or type(obj) in {TrafficLight, Lane, Street
}) and self.grid_square.intersects(obj):
typ = type(obj)
if typ == TrafficLight:
if obj.color == RED:
typ = "TrafficLight-Red"
elif obj.color == GREEN:
typ = "TrafficLight-Green"
elif obj.color == YELLOW:
typ = "TrafficLight-Yellow"
if typ not in collideable_map:
collideable_map[typ] = []
collideable_map[typ].append(obj)
self.all_collideables.append(obj)
for waypoint in car.waypoints:
collideable_map[Waypoint].append(waypoint)
self.all_collideables.append(waypoint)
terrain_window = pygame.Surface((self.grid_dim, self.grid_dim))
drivable_window = pygame.Surface((self.grid_dim, self.grid_dim))
undrivable_window = pygame.Surface((self.grid_dim, self.grid_dim))
car_window = pygame.Surface((self.grid_dim, self.grid_dim))
ped_window = pygame.Surface((self.grid_dim, self.grid_dim))
light_window_red = pygame.Surface((self.grid_dim, self.grid_dim))
light_window_green = pygame.Surface((self.grid_dim, self.grid_dim))
light_window_yellow = pygame.Surface((self.grid_dim, self.grid_dim))
direction_window = pygame.Surface((self.grid_dim, self.grid_dim))
direction_pixel_window \
= pygame.Surface((self.grid_dim, self.grid_dim))
direction_edge_window \
= pygame.Surface((self.grid_dim, self.grid_dim))
gd = self.grid_dim
a0 = self.car.angle + np.pi / 2
a1 = self.car.angle
rel = (self.car.x + gd / 2 * np.cos(a0) - gd / 6 * np.cos(a1),
self.car.y - gd / 2 * np.sin(a0) + gd / 6 * np.sin(a1),
self.car.angle)
for typ in [Terrain, Sidewalk, PedCrossing]:
for obj in collideable_map[typ]:
rel_obj = obj.get_relative(rel)
rel_obj.render(terrain_window, border=None)
for obj in collideable_map[Lane]:
rel_obj = obj.get_relative(rel)
if not car.can_collide(obj):
rel_obj.render(drivable_window, border=None)
else:
rel_obj.render(undrivable_window, border=None)
for obj in collideable_map[Street]:
rel_obj = obj.get_relative(rel)
rel_obj.render(drivable_window, border=None)
for obj in collideable_map[Car]:
rel_obj = obj.get_relative(rel)
rel_obj.render(car_window, border=None)
for obj in collideable_map[Pedestrian]:
rel_obj = obj.get_relative(rel)
rel_obj.render(ped_window, border=None)
for obj in collideable_map["TrafficLight-Red"]:
rel_obj = obj.get_relative(rel)
rel_obj.render(light_window_red, border=None)
for obj in collideable_map["TrafficLight-Green"]:
rel_obj = obj.get_relative(rel)
rel_obj.render(light_window_green, border=None)
for obj in collideable_map["TrafficLight-Yellow"]:
rel_obj = obj.get_relative(rel)
rel_obj.render(light_window_green, border=None)
point = (int(gd / 6), int(gd / 2))
edge_point = None
def is_on_screen(point, gd):
return 0 <= point[0] < gd and 0 <= point[1] < gd
line_width = 20
for p in self.car.waypoints:
relp = p.get_relative(rel)
new_point = int(relp.x), int(relp.y)
if not edge_point and is_on_screen(
point, gd) and not is_on_screen(new_point, gd):
edge_point = new_point
pygame.draw.line(direction_window, (255, 255, 255), point,
new_point, line_width)
point = new_point
if edge_point:
edge_point = (min(gd - 1, max(0, edge_point[0])),
min(gd - 1, max(0, edge_point[1])))
pygame.draw.circle(direction_pixel_window, (255, 255, 255),
edge_point, line_width)
if edge_point:
if edge_point[0] == 0:
pygame.draw.line(direction_edge_window, (255, 255, 255),
(0, 0), (0, gd - 1), line_width)
if edge_point[0] == gd - 1:
pygame.draw.line(direction_edge_window, (255, 255, 255),
(gd - 1, 0), (gd - 1, gd - 1), line_width)
if edge_point[1] == 0:
pygame.draw.line(direction_edge_window, (255, 255, 255),
(0, 0), (gd - 1, 0), line_width)
if edge_point[1] == gd - 1:
pygame.draw.line(direction_edge_window, (255, 255, 255),
(0, gd - 1), (gd - 1, gd - 1), line_width)
self.pygame_rep = [
pygame.transform.rotate(window, 90) for window in [
terrain_window, drivable_window, undrivable_window, car_window,
ped_window, light_window_red, light_window_green,
light_window_yellow, direction_window, direction_pixel_window,
direction_edge_window
]
]
def render(self, surface):
self.grid_square.render(surface, border=10)
if self.car.vis_level > 3:
if self.car.vis_level > 4:
for obj in self.all_collideables:
obj.render_debug(surface)
for y in range(4):
for x in range(2):
i = y + x * 4
if i < len(self.pygame_rep):
surface.blit(self.pygame_rep[i],
(surface.get_size()[0] - self.grid_dim *
(x + 1), self.grid_dim * y))
pygame.draw.rect(
surface, (200, 0, 0),
pygame.Rect(
(surface.get_size()[0] - self.grid_dim *
(x + 1) - 5, 0 - 5 + self.grid_dim * y),
(self.grid_dim + 10, self.grid_dim + 10)), 10)
def get_array(self):
arr = np.zeros((self.grid_dim, self.grid_dim, len(self.pygame_rep)))
for i in range(len(self.pygame_rep)):
arr[:, :, i] = pygame.surfarray.array2d(self.pygame_rep[i]) != 0
# print(pygame.surfarray.array2d(self.pygame_rep[i]) != 0)
if self.downsample:
arr = self.sp_imresize(arr, self.shape)
return arr
def sp_imresize(self, arr, shape):
return np.array(
[imresize(arr[:, :, i], shape) for i in range(arr.shape[2])]).T