def calc_rotation(self, pnt):
""" Calc the rotation of the triangle with another point"""
triangle_line = distance(self.center, self.top)
point_line = distance(self.center, pnt)
aux_line = distance(pnt, self.top)
if point_line == 0 or triangle_line == 0:
return 0
value = math.acos((point_line**2 - aux_line**2 + triangle_line**2) /
(2 * point_line * triangle_line)) * 180 / math.pi
m = slope(self.top, pnt)
b = offset_rect(self.top, m)
dx, dy = delta_x_y(m, b, self.center)
orientation = up_or_down(dx, dy, self.center, m)
return value * is_clockwise(m, orientation, self.top, pnt) * -1
def calc_rotation(self, pnt):
self.point = """ Calc the rotation of the triangle with another point"""
center = self.pos
top = (self.pos[0], self.pos[1] + self.separation)
triangle_line = distance(self.pos, top)
point_line = distance(self.pos, pnt)
aux_line = distance(pnt, top)
if point_line == 0 or triangle_line == 0:
return 0
value = math.acos((point_line**2 - aux_line**2 + triangle_line**2) /
(2 * point_line * triangle_line)) * 180 / math.pi
m = slope(top, pnt)
b = offset_rect(top, m)
dx, dy = delta_x_y(m, b, center)
orientation = up_or_down(dx, dy, center, m)
return value * is_clockwise(m, orientation, top, pnt) * -1
def calc_top_center(self):
""" Return the center and the top point of the triangle """
distance1 = distance(self.contours[0][0], self.contours[1][0])
distance2 = distance(self.contours[1][0], self.contours[2][0])
distance3 = distance(self.contours[0][0], self.contours[2][0])
minor = min(distance1, distance2, distance3)
center = top = (-1, -1)
if minor == distance1:
top = self.contours[2][0]
center = between_pt(self.contours[0][0], self.contours[1][0])
elif minor == distance2:
top = self.contours[0][0]
center = between_pt(self.contours[2][0], self.contours[1][0])
elif minor == distance3:
top = self.contours[1][0]
center = between_pt(self.contours[0][0], self.contours[2][0])
top = (top[0], top[1])
return top, center
def get_close_unknown_position(self, pos):
if self.new_pos[0] == -1 or distance(pos, self.new_pos) < 30:
new_pos = (-1, -1)
min_dist = 9999
contours, _ = cv2.findContours(
cv2.cvtColor(self.area, cv2.COLOR_BGR2GRAY), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_TC89_L1)
for cnt in contours:
area = cv2.contourArea(cnt)
approx = cv2.convexHull(cnt)
max_rand = len(cnt) - 1
rand = random.randint(0, max_rand)
x = cnt[rand][0][0]
y = cnt[rand][0][1]
dist = min(distance(pos, (x, y)), min_dist)
if dist < min_dist:
new_pos = (x, y)
min_dist = dist
self.new_pos = new_pos
return self.new_pos
def update(self, status, boxes, agent, ants: List):
''' update status of the claw '''
# closed
if (self.status == b'\x02') and (status == b'\x01'):
self.status = b'\x01'
self.separation -= 1
return boxes
elif self.status == b'\x01':
if self.separation > 10:
self.separation -= 1
for box in boxes:
distance_to_box = distance(box.pos, self.pos)
if distance_to_box < 80 or (box.leader is not None
and distance_to_box < 120):
self.status = b'\x04'
self.box_id = box.id
agent.send_box_data(box)
agent.con.writeCharacteristic(agent.chars.claw,
b'\x04',
withResponse=True)
agent.destination = (50, 50)
agent.con.writeCharacteristic(agent.chars.com,
b'\x3422',
withResponse=True)
if box.leader is None:
box.leader = self.agent
else:
self.leader = box.leader
for ant in ants:
if ant.color == box.leader:
ant.con.writeCharacteristic(ant.chars.claw,
b'\x06',
withResponse=True)
self.leader_pos = ant.triangle.center
break
elif distance_to_box < 160:
agent.destination = box.pos
agent.box_found = True
return boxes
if self.status == b'\x04':
boxes = self.update_box_pos(boxes)
return boxes