def add_walls(simulation):
# walls, yes circular walls...
wall_radius = 30
for x in range(0, simulation.size[0] + wall_radius, wall_radius):
for y in (0, simulation.size[1]):
simulation.things.append(
Obstacle(radius=wall_radius,
position=Vector2D(x, y))) # top and bottom wall
for y in range(0, simulation.size[1] + wall_radius, wall_radius):
for x in (0, simulation.size[0]):
simulation.things.append(
Obstacle(radius=wall_radius,
position=Vector2D(x, y))) # left and right wall
def build_robot():
# Build ROBOT C
robot = CircularRobot(AvoidanceStrategy(),
radius=5,
position=Vector2D(230, 200),
name="C")
robot.attach_sensor(
DistanceSensor("dist_front",
offset=Vector2D(3, 0),
angle_offset=0,
field_of_view_angle=np.deg2rad(15)))
return robot
def robotC():
# Build ROBOT C
robotC = CircularRobot(AvoidanceStrategy(),
radius=5,
position=Vector2D(200, 100),
name="C")
robotC.attach_sensor(
FieldIntensitySensor("left", field_type="light", offset=Vector2D(3,
3)))
robotC.attach_sensor(
FieldIntensitySensor("right",
field_type="light",
offset=Vector2D(3, -3)))
robotC.attach_sensor(
DistanceSensor("dist_front",
offset=Vector2D(3, 0),
angle_offset=0,
field_of_view_angle=np.deg2rad(15)))
return robotC
def is_in_field_of_view(self, thing):
angle_added_by_radius = np.tan(
(thing.radius / 2) /
thing.position.get_euclidean_distance(self.get_position()))
view_direction = normalize_angle(self.angle_offset + self.robot.angle)
vec_to_thing = Vector2D(x=None,
y=None,
data=thing.position.data - self.get_position())
angle_between = normalize_angle(view_direction -
vec_to_thing.get_angle())
angle_between = min(angle_between, 2 * PI - angle_between)
# todo handle negative angle betweens (over zero distance
return abs(
angle_between) <= self.field_of_view_angle + angle_added_by_radius
def __init__(self, position=Vector2D(0, 0), radius=0):
super().__init__(position=position, radius=radius)
self.type = "obstacle"
def __init__(self, name, field_type, offset=Vector2D(0, 0)):
super().__init__(name=name, offset=offset)
self.field_type = field_type
def __init__(self, position=Vector2D(0, 0), radius=0):
super().__init__(position, radius)
self.type = "marker"
def __init__(self, position=Vector2D(0, 0), radius=0):
self.position = position
self.radius = radius
def build_scenario():
simulation = Simulation(np.array([800, 500]))
# Build ROBOT A
robotA = CircularRobot(BraitenbergStrategy(robot_type=0),
radius=5,
position=Vector2D(120, 140),
name="A")
robotA.attach_sensor(
FieldIntensitySensor("left", field_type="light", offset=Vector2D(3,
3)))
robotA.attach_sensor(
FieldIntensitySensor("right",
field_type="light",
offset=Vector2D(3, -3)))
# Build ROBOT B
robotB = CircularRobot(BraitenbergStrategy(robot_type=1),
radius=5,
position=Vector2D(240, 100),
name="B")
robotB.attach_sensor(
FieldIntensitySensor("left", field_type="light", offset=Vector2D(3,
3)))
robotB.attach_sensor(
FieldIntensitySensor("right",
field_type="light",
offset=Vector2D(3, -3)))
robotB.attach_sensor(
DistanceSensor("dist_front",
offset=Vector2D(3, 0),
angle_offset=0,
field_of_view_angle=np.deg2rad(25)))
# Bulid ROBOT D
robotD = CircularRobot(AvoidanceTwoStrategy(),
radius=5,
position=Vector2D(200, 100),
name="D")
robotD.attach_sensor(
DistanceSensor("dist_left",
offset=Vector2D(3, -3),
angle_offset=-15,
field_of_view_angle=np.deg2rad(15)))
robotD.attach_sensor(
DistanceSensor("dist_right",
offset=Vector2D(3, 3),
angle_offset=15,
field_of_view_angle=np.deg2rad(15)))
# Add robots to the simulation
# simulation.things.append(robotA)
# simulation.things.append(robotB)
simulation.things.append(robotC())
# simulation.things.append(robotD)
# Add three light sources to the simulation
simulation.things.append(
FieldSource(5,
"light",
position=Vector2D(80, 80),
function=lambda x: -0.05 * x * x + 1000
if -0.05 * x * x + 1000 > 0 else 0))
simulation.things.append(
FieldSource(2,
"light",
position=Vector2D(240, 230),
function=lambda x: -5 * x + 1000
if -5 * x + 1000 > 0 else 0))
simulation.things.append(
FieldSource(2,
"light",
position=Vector2D(240, 20),
function=lambda x: -5 * x + 3000
if -5 * x + 3000 > 0 else 0))
def loop(simulation):
# pygame stuff
zoom = 2 # only 1, 2 or 4
screen = pygame.display.set_mode(simulation.size*zoom)
pause = True
root = tk.Tk()
root.withdraw()
try:
while True:
if not pause:
simulation.step()
screen.fill(black)
for thing in simulation.things:
try:
type = thing.type
except AttributeError:
continue
if type == "robot":
pygame.draw.circle(screen, red, thing.position.data.astype(int)*zoom, thing.radius*zoom, 2*zoom)
pygame.draw.line(
screen,
green,
thing.position.data.astype(int) * zoom,
(thing.position.data + Vector2D(thing.radius, 0).get_rotated(thing.angle)).astype(int) * zoom
)
for sensor in thing.sensors:
pygame.draw.circle(screen, white, sensor.get_position().astype(int)*zoom, 1*zoom, 1)
elif type == "field_source":
pygame.draw.circle(screen, green, thing.position.data.astype(int)*zoom, thing.radius*zoom, 2*zoom)
elif type == "obstacle":
pygame.draw.circle(screen, grey, thing.position.data.astype(int) * zoom, thing.radius * zoom, 2 * zoom)
elif type == "marker":
pygame.draw.circle(screen, green, thing.position.data.astype(int) * zoom, thing.radius * zoom,
2 * zoom)
pygame.display.flip()
time.sleep(0.01)
for event in pygame.event.get():
if event.type == pygame.QUIT: sys.exit()
if event.type == pygame.MOUSEBUTTONDOWN:
pos = pygame.mouse.get_pos()
simulation.things.append(
Obstacle(radius=NEW_OBSTACLE_SIZE, position=Vector2D(pos[0] / zoom, pos[1] / zoom)))
if event.type == pygame.KEYUP and event.key == pygame.K_p:
pause = not pause
if event.type == pygame.KEYUP and event.key == pygame.K_s:
# save simulation
simulation.save()
if event.type == pygame.KEYUP and event.key == pygame.K_l:
# load simulation from selectable file
simulation = Simulation.loadFrom(None)
except KeyboardInterrupt:
print("Stopped")