def __init__(self, start_wps=[], end_wps=[], **kwargs):
Shape.__init__(self, color=(0xf1, 0xf4, 0xf5), **kwargs)
point0 = (self.points[2] + self.points[3]) / 2
point1 = (self.points[0] + self.points[1]) / 2
if len(start_wps) and len(end_wps):
self.start_waypoints = start_wps
self.end_waypoints = end_wps
else:
self.start_waypoints = [
Waypoint(point0[0],
point0[1],
owner=self,
ydim=5,
angle=self.angle),
Waypoint(point1[0],
point1[1],
owner=self,
ydim=5,
angle=self.angle + np.pi)
]
self.end_waypoints = [
Waypoint(point1[0],
point1[1],
owner=self,
ydim=5,
angle=self.angle),
Waypoint(point0[0],
point0[1],
owner=self,
ydim=5,
angle=self.angle + np.pi)
]
self.start_waypoints[0].nxt = [self.end_waypoints[0]]
self.start_waypoints[1].nxt = [self.end_waypoints[1]]
def __init__(self, vel=0, mass=400, max_vel=5,
planning_depth=20, **kwargs):
from fluids.assets import Lane, Car, Pedestrian, TrafficLight, Terrain, Sidewalk, PedCrossing
collideables = [Lane,
Car,
Pedestrian,
TrafficLight,
Terrain,
Sidewalk,
PedCrossing]
Shape.__init__(self,
collideables=collideables,
color=(20, 150, 250),
xdim=70,
ydim=35,
**kwargs)
self.l_r = self.l_f = self.ydim / 2
self.mass = mass
self.max_vel = max_vel
self.vel = vel
self.waypoints = []
self.trajectory = []
self.planning_depth = planning_depth
self.PID_acc = PIDController(1.0, 0, 0)
self.PID_steer = PIDController(2.0, 0, 0)
self.last_action = SteeringAction(0, 0)
self.last_obs = None
self.last_distance = 0
self.last_to_goal = 0
self.stopped_time = 0
self.running_time = 0
self.last_blob_time = -1
self.cached_blob = self.get_future_shape()
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
def __init__(self, max_vel=2, vel=0, planning_depth=2, dim=25, **kwargs):
Shape.__init__(self, color=(0xF4, 0x80, 0x04), xdim=dim, ydim=dim, **kwargs)
self.max_vel = max_vel
self.vel = vel
self.waypoints = []
self.trajectory = []
self.planning_depth = planning_depth
def __init__(self, max_vel=2, vel=0, planning_depth=2, **kwargs):
Shape.__init__(self, color=(255, 250, 150), xdim=20, ydim=20, **kwargs)
self.max_vel = max_vel
self.vel = vel
self.waypoints = []
self.trajectory = []
self.planning_depth = planning_depth
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 __init__(self, start_wp=None, end_wp=None, wp_width=5, **kwargs):
# FCF59B
Shape.__init__(self, color=(0xFF, 0xFF, 0xFF), **kwargs)
if start_wp is not None and end_wp is not None:
self.start_waypoint = start_wp
self.end_waypoint = end_wp
else:
self.start_waypoint = (self.points[2] + self.points[3]) / 2
self.end_waypoint = (self.points[0] + self.points[1]) / 2
self.start_waypoint = Waypoint(
self.start_waypoint[0],
self.start_waypoint[1],
ydim=wp_width,
owner=self,
angle=self.angle,
)
self.end_waypoint = Waypoint(
self.end_waypoint[0],
self.end_waypoint[1],
ydim=wp_width,
owner=self,
angle=self.angle,
)
self.start_waypoint.nxt = [self.end_waypoint]
def __init__(self, vel=0, mass=400, max_vel=5,
planning_depth=13, path=-1, **kwargs):
from fluids.assets import Lane, Car, Pedestrian, TrafficLight, Terrain, Sidewalk, PedCrossing
collideables = [Car,
Pedestrian]
infractables = [Lane,
TrafficLight,
Terrain,
Sidewalk,
PedCrossing]
Shape.__init__(self,
collideables=collideables,
infractables=infractables,
color=(0x1d, 0xb1, 0xb0), # 769BB0
xdim=70,
ydim=35,
**kwargs)
self.l_r = self.l_f = self.ydim / 2
self.mass = mass
self.max_vel = max_vel
self.vel = vel
self.waypoints = []
self.trajectory = []
self.planning_depth = planning_depth
self.PID_acc = PIDController(1.0, 0, 0)
self.PID_steer = PIDController(2.0, 0, 0)
self.last_action = SteeringAccAction(0, 0)
self.last_obs = None
self.last_distance = 0
self.last_to_goal = 0
self.stopped_time = 0
self.running_time = 0
# Rewards metrics
self.last_four_positions = [np.asarray((self.x, self.y)),
np.asarray((self.x, self.y)),
np.asarray((self.x, self.y)),
np.asarray((self.x, self.y))]
self.jerk = calc_jerk(self.last_four_positions)
# Current reward
self.current_reward = 0.0
# Total reward
self.total_collisions = 0.0
self.total_infractions = 0.0
self.total_time = 0.0
self.total_liveliness = self.vel/self.max_vel
self.total_jerk = self.jerk
self.total_traj_follow = self.last_to_goal
self.total_reward = 0.0
self.last_blob_time = -1
self.cached_blob = self.get_future_shape()
self.path = path
self.keep_random = False
self.generated_first_traj = False
def __init__(self, **kwargs):
Shape.__init__(self, color=(50, 50, 50), **kwargs)
self.start_waypoint = (self.points[2] + self.points[3]) / 2
self.end_waypoint = (self.points[0] + self.points[1]) / 2
self.start_waypoint = Waypoint(self.start_waypoint[0],
self.start_waypoint[1], self.angle)
self.end_waypoint = Waypoint(self.end_waypoint[0],
self.end_waypoint[1], self.angle)
self.start_waypoint.nxt = [self.end_waypoint]
def __init__(self, **kwargs):
Shape.__init__(self, color=(120, 150, 20), **kwargs)
point0 = (self.points[2] + self.points[3]) / 2
point1 = (self.points[0] + self.points[1]) / 2
self.start_waypoints = [
Waypoint(point0[0], point0[1], self.angle),
Waypoint(point1[0], point1[1], self.angle + np.pi)
]
self.end_waypoints = [
Waypoint(point1[0], point1[1], self.angle),
Waypoint(point0[0], point0[1], self.angle + np.pi)
]
self.start_waypoints[0].nxt = [self.end_waypoints[0]]
self.start_waypoints[1].nxt = [self.end_waypoints[1]]
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 __init__(self, **kwargs):
Shape.__init__(self, color=(150, 200, 150), **kwargs)
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]
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
def __init__(self, **kwargs):
Shape.__init__(self, color=(30, 30, 30), **kwargs)
self.in_waypoints = []
self.out_waypoints = []
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
def __init__(self, **kwargs):
Shape.__init__(self, color=(0xFD, 0xF8, 0xEF), **kwargs)
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
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 __init__(self, **kwargs):
Shape.__init__(self, color=(0xff, 0xff, 0xff), **kwargs)
self.in_waypoints = []
self.out_waypoints = []
self.intersection_waypoints = []
def __init__(self, init_color="red", **kwargs):
color = {"red": RED, "green": GREEN}[init_color]
self.timer = {"red": 0, "green": 200}[init_color]
Shape.__init__(self, xdim=10, ydim=30, color=color, **kwargs)
def __init__(self, init_color="red", **kwargs):
color = {"red": RED, "green": GREEN, "yellow": YELLOW}[init_color]
self.timer = {"red": 0, "green": 200, "yellow": 350}[init_color]
Shape.__init__(self, xdim=20, ydim=60, color=color, **kwargs)
def __init__(self, **kwargs):
Shape.__init__(self, color=(0xfd,0xF8,0xef), **kwargs)
