def distance_sensor(self, walls: List[Wall]):
''' make lines around the agent and check if collide '''
# temp_sensor_lines = list()
start_object = None
end_object = None
self.collide = False
self.sensor_lines.clear()
for angle in range(0, 360, 30):
cart_pos = pol2cart(angle, 120)
cart_pos = (int(max(0, cart_pos[0] + self.xy[0])),
int(max(cart_pos[1] + self.xy[1], 0)))
cart_pos = (max(
min(cart_pos[0], self.recognition.screen_dimension[0] - 40),
40),
max(
min(cart_pos[1],
self.recognition.screen_dimension[1] - 40),
40))
temp_intercepts = False
for wall in walls:
intercepts, interception = wall.get_intersection(
[cart_pos, self.xy])
if intercepts:
if temp_intercepts:
first = distance(cart_pos, self.xy)
second = distance(interception, self.xy)
if first > second:
cart_pos = interception
else:
cart_pos = interception
temp_intercepts = True
if temp_intercepts:
if start_object is None:
start_object = cart_pos
else:
end_object = cart_pos
else:
if start_object is not None:
self.sensor_lines.append((start_object, "red"))
self.collide = True
if end_object is not None:
self.sensor_lines.append((end_object, "red"))
end_object = None
start_object = None
self.sensor_lines.append((cart_pos, "blue"))
def process_click(type: int, coord: tuple, env_process: EnvProcess,
window: Window):
x, y = coord
ant: Agent
x_offset = min(env_process.borders[1][0], env_process.borders[0][0])
y_offset = min(env_process.borders[1][1], env_process.borders[0][1])
for i, ant in enumerate(env_process.ants):
size = distance(ant.triangle.center, ant.triangle.top)
ax, ay = ant.xy
ax += x_offset
ay += y_offset
if (ax - (size / 2)) <= x <= (ax + (size / 2)) and (
ay - (size / 2)) <= y <= (ay + (size / 2)):
window["agent-column"].metadata = i
return
if (x_offset < x < max(env_process.borders[1][0],
env_process.borders[0][0]) and y_offset < y <
max(env_process.borders[1][1], env_process.borders[0][1])):
if window["GRAPH-BOX"].metadata == "activated":
if type is 2:
env_process.boxes.append(
Box((x - x_offset, y - y_offset), len(env_process.boxes)))
elif window["GRAPH-WALL"].metadata == "activated":
if type == 1:
window.metadata["tempPos"] = (x - x_offset, y - y_offset)
else:
env_process.walls.append(
Wall(window.metadata["tempPos"],
(x - x_offset, y - y_offset), len(env_process.walls)))
else:
env_process.pheromones.append(
Pheromone(x - x_offset, y - y_offset, 5, b"0x12"))
def update(self, triangle, time_since_last_update, pheromones, walls: Wall,
borders):
""" Update the sensors of the agent via BLE"""
# if triangle.is_valid() and self.triangle.is_valid():
# self.speed_rotation = distance(
# self.triangle.top, triangle.top) / time_since_last_update
self.triangle = triangle
# Calc speed
# prev_speed = self.speed
self.speed = distance(self.triangle.center,
self.xy) / time_since_last_update
# update Time
self.last_update = time.time()
# update position
# prev_position = self.xy
# new_position = self.triangle.center
if self.sending.pheromone != "none":
self.send_pheromones(
get_close_pheromones(12000, self.xy, pheromones))
self.sending.pheromone = "none"
self.read_message()
self.recognition.update_vision(self.triangle.center)
new_dest = self.recognition.get_close_unknown_position(
self.triangle.position)
if new_dest[
0] != -1 and self.claw.box_id == -1 and self.helping is False and self.destination != (
50,
50) and self.bugging is False and self.box_found is False:
new_dest = (max(40, min((borders[0] - 40), new_dest[0])),
max(40, min(borders[1] - 40, new_dest[1])))
self.destination = new_dest
# if abs(distance(prev_position, new_position)) > 6:
self.xy = self.triangle.center
self.send_rotation()
self.send_pos()
self.distance_sensor(walls)
time_to_update = time.time()
if self.collide and time_to_update - self.last_update_collision > 1 and self.claw.box_id != -1 and self.destination != (
50, 50):
self.send_distance_lines()
self.last_update_collision = time_to_update
time_to_update = time.time() - time_to_update
print(time_to_update)
# else:
# self.xy = prev_position
self.claw.pos = self.triangle.center
def draw_distance(self, frame, offset=(0, 0)):
""" Draw the distance with a circle and a line from top to pnt """
try:
if self.destination[0] != -1:
cv2.line(frame,
tuple(map(sum, zip(self.triangle.center, offset))),
tuple(map(sum, zip(self.triangle.top, offset))),
(200, 150, 50), 2)
cv2.putText(
frame,
('%.2f' %
(distance(self.triangle.center, self.destination) / 5)) +
" cm", self.destination, FONT, 1, 255)
except AttributeError:
pass
def get_close_pheromones(dist: int, pos: tuple, pheromones: List[Pheromone]):
close_pheromones = list(
filter(
lambda pheromone: distance(pos, (pheromone.x, pheromone.y)) < dist,
pheromones))
return close_pheromones